#include "joint.h"
#include "math.h"
#include "angle_encode.h"
#include "aos/kv.h"
#include "stdlib.h"
#include "string.h"
#include "wheel.h"
#include "linux_lidar.h"
#include "upper.h"
#include "safty.h"
#include "lidar_navigation.h"

static Type_Link_Def Link;

//任务线程
/********************************************************************************/
void xp_joint_thread(void *arg);
void xp_joint_motion_thread(void *arg);
//配置参数相关
/********************************************************************************/
char *xp_joint_get_length_strKey(uint8_t id);
int xp_joint_save_params(char *pKeyStr, int32_t val);
int32_t xp_joint_read_creat_params(char *pKeyStr, int32_t defaultValue);
//工具类函数
/********************************************************************************/
float caclAngle(Type_Vector3_Def const * pSrcPose, Type_Vector3_Def const * pDesPose);
float caclDistance(Type_Vector3_Def const * pSrcPose, Type_Vector3_Def const * pDesPose);
float xp_cacl_point_angle(float src_x, float src_y, float des_x, float des_y);
float xp_cacl_point_dist(float src_x, float src_y, float des_x, float des_y);
float limit_float(float val, float min, float max);
int32_t limit_int32(int32_t val, int32_t min, int32_t max);
float xp_pid_cacl(Type_Pid_Def * const pid, float err);
float xp_joint_cacl_dist_point_to_line(Type_Vector3_Def const * pPoint, Type_Vector3_Def const * pLinePoint1, Type_Vector3_Def const * pLinePoint2);
void xp_joint_read_params(void);
//功能函数相关
/********************************************************************************/
int xp_joint_update_pos(uint8_t type);
int xp_set_state(Type_MotionState_Enum state);
const char *xp_get_state_str(Type_MotionState_Enum state);
Type_List_Def * MotionPlan(Type_Vector3_Def * pTarPose, Type_MotionOption_Enum option);
int xp_joint_get_area(Type_Vector3_Def const * pose);
float xp_joint_get_angle_diff(float angle1, float angle2);
int xp_joint_posture_adjust(float tarAngle);
int xp_joint_go_pose(Type_Vector3_Def const * pTarPose);
int xp_move_pid(float dist_err, float angle_err);
int xp_joint_move_to_point(int32_t tar_x, int32_t tar_y, float tar_angle);
int xp_joint_get_wheel_encode(uint64_t encBuf[2]);
int xp_joint_get_wheel_encode_inc(int64_t errBuf[2]);
void joint_robot2_obstacle_update(Type_Vector3_Def const * probot2Pose);
uint8_t xp_joint_electric_fence_check_polling(void);
uint8_t xp_joint_over_range_check(void);
float IkCacl(Type_Vector3_Def const *pTarPose, Type_Vector3_Def Points[]);
bool xp_joint_calibration_link(Type_Vector3_Def * const pTarPose);
void xp_joint_print_efence_state(uint8_t state);
bool xp_joint_enc_unusual_check(void);
void Predict_robot_state_by_speed(float vl, float vr, float dt);

//坐标系转换相关
/********************************************************************************/
Type_Vector3_Def * const xp_joint_coordinate_transform_from_world_to_robot(Type_Vector3_Def const * pSrcPoint, Type_Vector3_Def * const pDesPoint);
Type_Vector3_Def * const xp_joint_coordinate_transform_from_robot_to_world(Type_Vector3_Def const * pSrcPoint, Type_Vector3_Def * const pDesPoint);
Type_Vector3_Def * const xp_joint_coordinate_transform_from_lidar_to_robot(Type_Vector3_Def const * pSrcPoint, Type_Vector3_Def * const pDesPoint);
Type_Vector3_Def * const xp_joint_coordinate_transform_from_lidar_to_world(Type_Vector3_Def const * pSrcPoint, Type_Vector3_Def * const pDesPoint);
Type_Vector3_Def * const xp_joint_coordinate_transform(Type_Vector3_Def const * pSrcPoint, Type_Vector3_Def * const pDesPoint, Type_Vector3_Def const * offset);
//路径规划相关
/*************************/
void AStar_Test(void);
void mapInit_hasCar();

Type_List_Def * const newList(void);
void ListDistroy(Type_List_Def * pList);
void PrintList(Type_List_Def * const pList);
Type_List_Def * const ListReverse(Type_List_Def * const pList);
bool List_Add(Type_List_Def * const pList, Type_Node_Def * const pNode, bool (*isMatchSortRules_CB)(void const * pListNode, void const * pNode));
bool List_Remove(Type_List_Def * const pList, Type_Node_Def const * pNode, bool (*isMatchRemoveRules_CB)(void const * pListNode, void const * pNode));
Type_Node_Def * const List_Find(Type_List_Def const * pList, Type_Node_Def const * pNode, bool (*isMatchFindRules_CB)(void const * pListNode, void const * pNode));
Type_Node_Def * List_ReadFromHead(Type_List_Def * const pList, Type_Node_Def * const pRetNode);
Type_Node_Def * List_ReadbyId(Type_List_Def * const pList, uint16_t id);

Type_Node_Def * const newNode(uint16_t x, uint16_t y);
Type_Node_Def * const newNodeCopy(Type_Node_Def const * pNode);
uint8_t *WorldPosToMapPtr(int x, int y);
uint8_t *WorldPosToMapPtr2(Type_Vector3_Def const *pPose);
bool IsValidNode(int x, int y);
bool IsValidNode2(Type_Node_Def const *pNode);
bool isValidWordPose(int x, int y);
bool isValidWordPose2(Type_Vector3_Def const *pPose);

void DilationMap(int radius);
Type_List_Def* SeachPathAStar(Type_Vector3_Def * pStartPose, Type_Vector3_Def * pEndPose);
Type_Node_Def * const FindMinFCostNode(Type_List_Def const * pList);
Type_List_Def * GeneratePath(Type_Node_Def * const pStartNode, Type_Node_Def * const pEndNode);
Type_List_Def * GetNeighor(Type_Node_Def const * pNode, Type_List_Def const * pCloseList);
int GetDistance(Type_Node_Def const * pNode1, Type_Node_Def const * pNode2);

Type_Vector3_Def * MapPosToWorldPos(Type_Node_Def * const pNode, Type_Vector3_Def * const pVector);
Type_Node_Def *WorldPosToMapNode(Type_Node_Def * const pNode, Type_Vector3_Def const * pVector);
bool SortRule_F_from_small_to_big(void const * pListNode, void const * pNode);
bool isEqualRules(void const * pListNode, void const * pCurNode);

extern int32_t xp_set_lidar_mode_before_joint_move(int32_t cur_lidar_mode);
/********************************************************************************/
bool isMotionThreadIdle = false;

//状态字符串数组
const char StateStrList[][8] = {"IDLE", "PLAN", "MOVING", "POSTURE", "PAUSE", "RESUME", "STOP"};
//电子围栏状态字符串数组
const uint8_t efenceStrList[][10] = {"LF", "LB", "RB", "RF", "RAD_MIN", "RAD_MAX", "ENC3_MIN", "ENC3_MAX"};
//此数组分别为机器人 左前 左后 右后 右前轮廓角点在场地坐标系上的坐标
Type_Vector3_Def RobotFramePointArray[4];
//场地角点坐标数组
const Type_Vector3_Def AreaPointArray[4] = {
    { (DEF_AREA_MIN_X + ROBOT_LENGTH) / 4, -(DEF_AREA_MIN_Y + ROBOT_LENGTH) / 4, 45 },
    { (DEF_AREA_MIN_X + ROBOT_LENGTH) / 4,  (DEF_AREA_MIN_Y + ROBOT_LENGTH) / 4, 135},
    {-(DEF_AREA_MIN_X + ROBOT_LENGTH) / 4,  (DEF_AREA_MIN_Y + ROBOT_LENGTH) / 4, 225},
    {-(DEF_AREA_MIN_X + ROBOT_LENGTH) / 4, -(DEF_AREA_MIN_Y + ROBOT_LENGTH) / 4, 315}
};
int mallocCount = 0;
void * xp_joint_memery_malloc(uint16_t size)
{
	mallocCount++;
	void *p = aos_malloc(size);
	//print("malloc <0x%04X %d Byte> %d\r\n", p, size, mallocCount);
	return p;
}
void xp_joint_memery_free(void *mem)
{
	mallocCount--;
	//print("free <0x%04X> %d\r\n", mem, mallocCount);
	aos_free(mem);
}
void xp_joint_map_init(void)
{
    //根据场地大小进行地图行列数初始化,最后的加一非常必要,否则会将场地坐标正x或正y最大值视为场地之外坐标
	Link.MAP_ROWS = (int)(Link.areaMaxLimit_P.x - Link.areaMaxLimit_N.x) / MAP_GRID_WIDTH + 1;		//地图栅格行数
	Link.MAP_COLUMNS = (int)(Link.areaMaxLimit_P.y - Link.areaMaxLimit_N.y) / MAP_GRID_WIDTH + 1;	//地图栅格列数
	
	//地图二维数组内存申请 
	/*****************************************************************************/
	//行指针,
	Link.Map = (uint8_t **)xp_joint_memery_malloc(sizeof(uint8_t *) * Link.MAP_ROWS );
	if (Link.Map){
		//列空间
		Link.Map[0] = (uint8_t *)xp_joint_memery_malloc(sizeof(uint8_t) * Link.MAP_ROWS * Link.MAP_COLUMNS );
		if (Link.Map[0]){
			for (uint16_t i = 0; i < Link.MAP_ROWS; i++){
				Link.Map[i] = Link.Map[0] + i * Link.MAP_COLUMNS;
			}
			print("Link.Map init success. Rows:%d Columns: %d\r\n", Link.MAP_ROWS, Link.MAP_COLUMNS);
		}else{	
			xp_joint_memery_free(Link.Map);
			print("Link.Map init failed\r\n");
			//return -1;
		}
	}else{
		print("Link.Map init failed\r\n");
		//return -1;
	}
}
int xp_joint_init(void)
{	
	int ret;
	
	//读取通过KV储存的参数
	xp_joint_read_params();
	
	//通过读取的场地范围申请地图内存
	xp_joint_map_init();
	
	//初始化为有车时的地图
	mapInit_hasCar();
	
	//初始化角度编码器
	xp_angleEncode_init();
	
	Link.Pid_angle.kp = 0.003f;
    Link.Pid_angle.ki = 0.0004f;
    Link.Pid_angle.kd = 0.0001f;
    Link.Pid_angle.sum_limit = 0.08f;
    Link.Pid_angle.out_limit = 0.15f;

    Link.Pid_dist.kp = 0.001f;
    Link.Pid_dist.ki = 0.0001f;
    Link.Pid_dist.kd = 0.0001f;
    Link.Pid_dist.sum_limit = 0.05f;
    Link.Pid_dist.out_limit = 0.2f;

    Link.posAllowError = 100;
    Link.angleAllowError = 1;
    Link.motorEn = true;
    Link.safeEn = true;
    Link.safeDist = 65;//安全避障距离默认65cm

    Link.logTime = 3000;
	Link.ForwardDist = 600;
	Link.millage = 0;

	//关节位置计算线程
	if (0 != aos_task_new("xp_joint_thread", xp_joint_thread, NULL, 2048)){
		return -1;
	}
	//机器人移动控制线程
	if (0 != aos_task_new("xp_joint_motion_thread", xp_joint_motion_thread, NULL, 2048)){
	    return -1;
	}
	return 0;
}

void xp_joint_thread(void *arg)
{
	bool encUpdateOk = false;
	uint64_t timeStamp = aos_now_ms();
	int ret;
	uint8_t state = 0;
	
	while(1){
		//读取管线架编码器角度
		/****************************************************************************/
		encUpdateOk = false;
		if (0 == (ret = xp_enc_read_all_encode())){
			for (uint8_t i = 0; i < JOINT_NUM; i++){
				if (0 == xp_angleEncode_get_angle_by_id(i, &Link.joints[i].pose.z)){
					encUpdateOk = true;
				}
			}
		}else{
			println("Warning! xp_enc_read_all_encode() ret <%d>\r\n", ret);
		}
		
		//当管线架编码器读取失败时,切换到通过左右轮编码器的ODOM方式定位
		if (encUpdateOk && !Link.odomEn){
			state = 0;
		}else if (0 == state){
			//state = 1;
            state = 3;  //编码器读取失败后不切换到里程计
		}
		
		//当管线架编码器读取成功,但变化异常时切换到通过左右轮编码器的ODOM方式定位
		//Todo... 
		if (0 != xp_joint_enc_unusual_check()){
			//state = 1;	//这里先对定位异常做一个检测,看看成功率,以后再写具体措施
		}
		
		//根据读取状态选择定位方式
		/****************************************************************************/
		switch(state){
			case 0:
				Link.odomContinueTime = 0;
				xp_joint_update_pos( USED_ENC );
			break;
			
			case 1:
				if (0 == xp_joint_get_wheel_encode((uint64_t *)Link.LastEncodeBuf)){
					state = 2;
					print("pose used wheel encode\r\n");
				}
			break;
			
			case 2:
				Link.odomContinueTime += aos_now_ms() - Link.poseUpdateTimeStamp;
				xp_joint_update_pos( USED_ODOM );
			break;
            
            default:
                break;
		}

		//仅在设置了log时间后才启用打印输出
		if (Link.logTime && aos_now_ms() - timeStamp > Link.logTime){
			timeStamp = aos_now_ms();
			print("joints_cur");
			for (uint8_t i = 0; i < JOINT_NUM; i++){
				print(" [%.0f %.0f %.1f] ", Link.joints[i].pose.x, Link.joints[i].pose.y, Link.joints[i].pose.z);
			}
			print(" mill: %ld %.1f\r\n", (int32_t)Link.millage, Link.robotPose.z);
			
			print("joints_pre");
			for (uint8_t i = 0; i < JOINT_NUM; i++){
				print(" [%.0f %.0f %.1f] ", Link.joints[i].nextPose.x, Link.joints[i].nextPose.y, Link.joints[i].nextPose.z);
			}
			print("  %.1f \r\n", Link.robotNextPose.z);
		}
		aos_msleep(200);
	}
}
bool xp_joint_enc_unusual_check(void)
{
    if (caclDistance(&Link.joints[2].pose, &Link.joints[2].nextPose) > 1000){
        println("Enc maybe is broken, predict pose is (%d, %d, %d) cur is (%d, %d, %d)\r\n",    (int)Link.joints[2].nextPose.x, \
                                                                                                (int)Link.joints[2].nextPose.y, \
                                                                                                (int)Link.joints[2].nextPose.z, \
                                                                                                (int)Link.joints[2].pose.x, \
                                                                                                (int)Link.joints[2].pose.y, \
                                                                                                (int)Link.joints[2].pose.z);
        return 1;
    }
    return 0;
}
bool sliderWindowFilter(Type_List_Def * pList, int step)
{
	Type_Vector3_Def vector = {0};
	Type_Node_Def *pNode = NULL;
	
	for (int i = 1; i < pList->count - 1; i++) {
		int sum_x = 0, sum_y = 0;
		int index = 0;
		for (int j = -step / 2; j < step / 2 + step % 2; j++) {
			if (i + j >= 0 && i + j < pList->count) {
				pNode = List_ReadbyId(pList, i + j);
				if (!pNode){
					return false;
				}
				sum_x += pNode->x;
				sum_y += pNode->y;
				index++;
			}
		}
		pNode = List_ReadbyId(pList, i);
		if (!pNode){
			return false;
		}
		pNode->x = sum_x / index;
		pNode->y = sum_y / index;
	}
}
void xp_joint_motion_thread(void *arg)
{
	Type_Node_Def node;
	int32_t lidar_dist = 0;
	uint32_t lidar_err_cnt = 0;
	uint32_t lidar_ok_cnt = 0;
	uint32_t obstacle_cnt = 0;
	bool lidar_err_flag = false;
	bool b_obstacle_flag = false;
	long long last_blow_time = 0;//上次吹气时刻
	uint8_t blow_count = 0;//连续吹气次数
	SAFTY_STATE_E safe_state = SAFTY_state_lidar_abnormal;
	int32_t lidar_mode_back = 0;
	
	while(1)
	{
		switch (Link.motionState) {
			case STA_IDLE:
				
			break;

			case STA_PLAN:
				if (Link.pPathList){
					//当路径list非空时,对其路径点进行平滑
					print("sliderWindowFilter %s\r\n", sliderWindowFilter(Link.pPathList, 5) ? ("OK") : ("faild"));
					if (List_ReadFromHead(Link.pPathList, &node)){
						MapPosToWorldPos(&node, &Link.currentTarPose); 
					}
				}else{
					//当路径list空,则直接执行点到点移动
					Link.currentTarPose = Link.finallyTarPose;
				}
				xp_set_state(STA_MOVING);
				//移动前将雷达算法切换成测距，并保存之前的雷达算法用于移动完之后恢复
				lidar_mode_back = xp_linux_lidar_doAngle_get();
				xp_set_lidar_mode_before_joint_move(lidar_mode_back);
			break;

			case STA_MOVING:
				while ( caclDistance(&Link.robotPose, &Link.currentTarPose) < Link.ForwardDist ){
					 if (List_ReadFromHead(Link.pPathList, &node)){
						MapPosToWorldPos(&node, &Link.currentTarPose); 
					 }else{
						break;
					 }
				}

				if (0 != xp_joint_go_pose(&Link.currentTarPose)) {
					//位置移动结束后进一步判断是否达到最终位置(虽有前瞻距离,但线程延迟可能导致机器到达给定点)
					if ( caclDistance(&Link.robotPose, &Link.finallyTarPose) < Link.posAllowError ){
						xp_set_state(STA_POSTURE);
					}
				}
				if (Link.isFirstSwitch) {
					Link.motionDistance = caclDistance(&Link.robotPose, &Link.currentTarPose ) + 1;	//加1防止除零
					Link.isFirstSwitch = false;
				}
				break;
			break;

			case STA_POSTURE:
				if (0 != xp_joint_posture_adjust(Link.finallyTarPose.z)) {
					xp_set_state(STA_STOP);
				}
			break;

			case STA_PAUSE:
				if (Link.isFirstSwitch) {
					Link.isFirstSwitch = false;
					Link.vl = 0;
					Link.vr = 0;
					if (Link.motorEn){
						xp_wheel_set_wheel_speed(0, 0);
					}
					print("motion pause\r\n");
				}else{
					if (0 == xp_joint_get_robot_pose(NULL, NULL, NULL) && 
						(SAFTY_state_normal == safe_state || SAFTY_state_too_near == safe_state)){
						//管线架编码器无异常，并且雷达避障无异常，则恢复移动
						xp_set_state(STA_RESUME);
					}
				}
			break;

			case STA_RESUME:
				xp_set_state(Link.lastMotionState);
				print("motion resume\r\n");
			break;

			case STA_STOP:
				Link.vl = 0;
				Link.vr = 0;
				Link.Pid_angle.sum = 0;
				Link.Pid_dist.sum = 0;
			
				if (Link.motorEn){
					xp_wheel_set_wheel_speed(0, 0);
				}
				
				if (Link.pPathList){
					ListDistroy(Link.pPathList);
					Link.pPathList = NULL;
				}

				print("motion complete\r\n");
				xp_set_state(STA_IDLE);
				//移动完之后将雷达算法切换回之前的模式
				if(lidar_mode_back != LIDAR_MODE_CAR_PROFILE){
					println("joint_move complete, lidar_mode %d -> %d", xp_linux_lidar_doAngle_get(), lidar_mode_back);
					xp_linux_lidar_doAngle_set(lidar_mode_back);
				}
			break;

			case STA_FAULT:

			break;
		}

		//判断管线架编码器是否正常
		if (xp_joint_get_robot_pose(NULL, NULL, NULL) && STA_IDLE != Link.motionState){
			xp_set_state(STA_PAUSE);
		}

        /***********************************************************************/
		//进行雷达是否正常工作的判断，以及安全避障的判断
		if(Link.safeEn && (STA_IDLE != Link.motionState) && (lidar_mode_back != LIDAR_MODE_CAR_PROFILE)){
			if(STA_POSTURE == Link.motionState){
				//原地旋转时采用小ROI的测量距离
				safe_state = xp_safty_check(LIDAR_roi_leave, Link.safeDist);
			}else{
				safe_state = xp_safty_check(LIDAR_roi_obstacle, Link.safeDist);
			}

			if(SAFTY_state_lidar_abnormal == safe_state){
				//雷达距离获取不到时暂停移动
				if(STA_MOVING == xp_joint_get_motion_state() || STA_POSTURE == xp_joint_get_motion_state()){
					xp_set_state(STA_PAUSE);
				}
			}else if(SAFTY_state_obstacle == safe_state){
				//雷达距离遇障时暂停移动
				if(STA_MOVING == xp_joint_get_motion_state() || STA_POSTURE == xp_joint_get_motion_state()){
					xp_set_state(STA_PAUSE);
				}
			}else{
				//雷达距离正常，在上面的switch状态机中恢复之前的移动
			}
		}
        /***********************************************************************/

		isMotionThreadIdle = true;
		aos_msleep(200);
		isMotionThreadIdle = false;
	}
}

void xp_joint_read_params(void)
{
	//读取连杆长度参数
	Link.joints[0].length = xp_joint_read_creat_params( "jot_linkLen0", 2000);
	Link.joints[1].length = xp_joint_read_creat_params( "jot_linkLen1", 2600);
	Link.joints[2].length = xp_cacl_point_dist(0, 0, ROBOT_CENTER_OFFSET_X, ROBOT_CENTER_OFFSET_Y);
	//读取待机位置参数
	Link.homePose.x = xp_joint_read_creat_params( "jot_homePos_x", DEFAULT_HOME_POSE_X);
	Link.homePose.y = xp_joint_read_creat_params( "jot_homePos_y", DEFAULT_HOME_POSE_Y);
	Link.homePose.z = xp_joint_read_creat_params( "jot_homePos_z", DEFAULT_HOME_POSE_Z);
	//读取场地最大范围限制参数
	Link.areaMaxLimit_P.x = xp_joint_read_creat_params( "jot_areaMaxLimit_Px",  DEF_AREA_MAX_X / 2);
	Link.areaMaxLimit_P.y = xp_joint_read_creat_params( "jot_areaMaxLimit_Py",  DEF_AREA_MAX_Y / 2);
	Link.areaMaxLimit_N.x = xp_joint_read_creat_params( "jot_areaMaxLimit_Nx", -DEF_AREA_MAX_X / 2);
	Link.areaMaxLimit_N.y = xp_joint_read_creat_params( "jot_areaMaxLimit_Ny", -DEF_AREA_MAX_Y / 2);
	
	Link.areaMinLimit_P.x = xp_joint_read_creat_params( "jot_areaMinLimit_Px",  DEF_AREA_MIN_X / 2);
	Link.areaMinLimit_P.y = xp_joint_read_creat_params( "jot_areaMinLimit_Py",  DEF_AREA_MIN_Y / 2);
	Link.areaMinLimit_N.x = xp_joint_read_creat_params( "jot_areaMinLimit_Nx", -DEF_AREA_MIN_X / 2);
	Link.areaMinLimit_N.y = xp_joint_read_creat_params( "jot_areaMinLimit_Ny", -DEF_AREA_MIN_Y / 2);
	//管线架角度范围限制
	Link.minRadiusLimit = xp_joint_read_creat_params( "jot_radius_min", DEF_RADIUS_LIMIT_MIN);
	Link.maxRadiusLimit = xp_joint_read_creat_params( "jot_radius_max", DEF_RADIUS_LIMIT_MAX);
	Link.encode3LimitMin = xp_joint_read_creat_params( "jot_enc3_min", DEF_ENC3_LIMIT_MIN);
	Link.encode3LimitMax = xp_joint_read_creat_params( "jot_enc3_max", DEF_ENC3_LIMIT_MAX);
	
	Link.Efence_en = xp_joint_read_creat_params( "jot_Efence_en", true);
	
	print("***********************************");
    print("Joint lens %d, %d, %d\r\n", Link.joints[0].length, Link.joints[1].length, Link.joints[2].length);
    print("Efence_en %d\r\n", Link.Efence_en);
	print("Link length: <0,1,2> - (%d, %d, %d)\r\n", Link.joints[0].length, Link.joints[1].length, Link.joints[2].length);
	print("Home Pose: [%d, %d, %d]\r\n", (int)Link.homePose.x, (int)Link.homePose.y, (int)Link.homePose.z);
	print("Area Max Limit: P-[%d, %d]  N-[%d, %d]\r\n", (int)Link.areaMaxLimit_P.x, (int)Link.areaMaxLimit_P.y, (int)Link.areaMaxLimit_N.x, (int)Link.areaMaxLimit_N.y);
	print("Area Min Limit: P-[%d, %d]  N-[%d, %d]\r\n", (int)Link.areaMinLimit_P.x, (int)Link.areaMinLimit_P.y, (int)Link.areaMinLimit_N.x, (int)Link.areaMinLimit_N.y);
	print("Radius limit range (%d ~ %d)\r\n", Link.minRadiusLimit, Link.maxRadiusLimit);
	print("Enc 3 limit range (%d ~ %d)\r\n", Link.encode3LimitMin, Link.encode3LimitMax);
	print("***********************************");
}

//切换状态
int xp_set_state(Type_MotionState_Enum state)
{
	Type_MotionState_Enum CurrState = Link.motionState;

	if (Link.motionState != state) {

		/*状态迁移图*/

		if (STA_IDLE == Link.motionState && STA_PLAN == state) {    			//空闲->规划
			Link.motionState = STA_PLAN;
		}
		else if (STA_IDLE == Link.motionState && STA_POSTURE == state) {        //空闲->位姿调整
			Link.motionState = STA_POSTURE;
		}
		else if (STA_PLAN == Link.motionState && STA_MOVING == state) {         //规划->移动
			Link.motionState = STA_MOVING;
		}
		else if (STA_PLAN == Link.motionState && STA_POSTURE == state) {        //规划->位姿调整
			Link.motionState = STA_POSTURE;
		}
		else if (STA_MOVING == Link.motionState && STA_POSTURE == state) {      //移动->位姿调整
			Link.motionState = STA_POSTURE;
		}
		else if (STA_MOVING == Link.motionState && STA_PLAN == state) {         //移动->规划
			Link.motionState = STA_PLAN;
		}
		else if (STA_POSTURE == Link.motionState && STA_PLAN == state) {        //位姿调整->规划
			Link.motionState = STA_PLAN;
		}
		else if (STA_PAUSE == Link.motionState && STA_RESUME == state) {        //暂停->恢复
			Link.motionState = STA_RESUME;
		}
		else if (STA_STOP == Link.motionState && STA_IDLE == state) {           //停止->空闲
			Link.motionState = STA_IDLE;
		}
		else if (STA_STOP == Link.motionState && STA_PLAN == state) {           //停止->规划
			Link.motionState = STA_PLAN;
		}
		else if (STA_PAUSE == state) {                                          //任意状态->暂停
			Link.motionState = STA_PAUSE;
		}
		else if (STA_STOP == state) {                                           //任意状态->停止
			Link.motionState = STA_STOP;
		}
		else if (STA_RESUME == Link.motionState) {                       		//恢复->任意状态
			Link.motionState = state;
		}
		
		else {
			println("Switch not allowed, %s -> %s\r\n", xp_get_state_str(CurrState), xp_get_state_str(state));
			return -1;              //不允许的状态切换
		}
		if (STA_PAUSE != CurrState) {	//保存暂停之前的状态
			Link.lastMotionState = CurrState;
		}
		Link.isFirstSwitch = true;
		println("State switch OK, %s -> %s\r\n", xp_get_state_str(CurrState), xp_get_state_str(state));
		return 0;                   //状态切换成功
	}
	else {
		println("Already in %s state\r\n", xp_get_state_str(CurrState));
		return 0;                   //已是当前状态
	}
}

Type_List_Def * MotionPlan(Type_Vector3_Def * pTarPose, Type_MotionOption_Enum option)
{
	Type_List_Def *pList = newList();

	if (NULL == pList){
		return NULL;
	}
	
	int tarArea = xp_joint_get_area(pTarPose);
	if (-1 == tarArea) {
		return pList;
	}

	int robotArea = xp_joint_get_area(&Link.robotPose);
	if (-1 == robotArea) {
		return pList;
	}

	List_Add(pList, newNode(Link.robotPose.x, Link.robotPose.y), NULL);

	if (OPTION_DIRECT_CCW == option) {           //逆时针移动方案
		int step_num_ccw = tarArea > robotArea ? (tarArea - robotArea) : (4 - robotArea + tarArea);
		int robot_dist = caclDistance(&Link.robotPose, &AreaPointArray[(robotArea + 1) % 4]);
		int tar_dist = caclDistance(pTarPose, &AreaPointArray[(robotArea + 1) % 4]);
		if (!(4 == step_num_ccw && robot_dist > tar_dist)) {
			for (int i = 0; i < step_num_ccw; i++) {
				List_Add(pList, newNode(AreaPointArray[(robotArea + i + 1) % 4].x, AreaPointArray[(robotArea + i + 1) % 4].y), NULL);
			}
		}
	}
	else if (OPTION_DIRECT_CW == option) {       //顺时针移动方案
		int step_num_cw = tarArea < robotArea ? (robotArea - tarArea) : (4 - tarArea + robotArea);
		int robot_dist = caclDistance(&Link.robotPose, &AreaPointArray[robotArea]);
		int tar_dist = caclDistance(pTarPose, &AreaPointArray[robotArea]);
		if (!(4 == step_num_cw && robot_dist > tar_dist)) {
			for (int i = 0; i < step_num_cw; i++) {
				List_Add(pList, newNode(AreaPointArray[(robotArea - i + 4) % 4].x, AreaPointArray[(robotArea - i + 4) % 4].y), NULL);
			}
		}
	}
	else if (OPTION_DIRECT_AUTO == option) {     //距离最近移动方案

		int step_num_ccw = tarArea >= robotArea ? (tarArea - robotArea) : (4 - robotArea + tarArea);
		int step_num_cw = tarArea <= robotArea ? (robotArea - tarArea) : (4 - tarArea + robotArea);

		if (step_num_ccw > step_num_cw) {               //逆时针方案所跨区域大于顺时针方案,
			for (int i = 0; i < step_num_cw; i++) {
				List_Add(pList, newNode(AreaPointArray[(robotArea - i + 4) % 4].x, AreaPointArray[(robotArea - i + 4) % 4].y), NULL);
			}
		}
		else if (step_num_ccw < step_num_cw) {          //逆时针方案所跨区域小于顺时针方案,旋转逆时针移动方案
			for (int i = 0; i < step_num_ccw; i++) {
				List_Add(pList, newNode(AreaPointArray[(robotArea + i + 1) % 4].x, AreaPointArray[(robotArea + i + 1) % 4].y), NULL);
			}
		}
		else {                                          //顺逆时针方案所跨区域皆相等时,计算各自的移动距离来判断顺逆方案
			float dist_ccw_sum = 0;
			dist_ccw_sum += caclDistance( &Link.robotPose, &AreaPointArray[(robotArea + 1) % 4]);
			dist_ccw_sum += caclDistance(  pTarPose, &AreaPointArray[(tarArea) % 4]);
			dist_ccw_sum += caclDistance( &AreaPointArray[(robotArea + 1) % 4], &AreaPointArray[(tarArea) % 4]);

			float dist_cw_sum = 0;
			dist_cw_sum += caclDistance( &Link.robotPose, &AreaPointArray[(robotArea) % 4]);
			dist_cw_sum += caclDistance( pTarPose, &AreaPointArray[(tarArea + 1) % 4]);
			dist_cw_sum += caclDistance( &AreaPointArray[(robotArea) % 4], &AreaPointArray[(tarArea + 1) % 4]);

			if (dist_ccw_sum > dist_cw_sum) {
				for (int i = 0; i < step_num_cw; i++) {
					List_Add(pList, newNode(AreaPointArray[(robotArea - i + 4) % 4].x, AreaPointArray[(robotArea - i + 4) % 4].y), NULL);
				}
			}
			else {
				for (int i = 0; i < step_num_ccw; i++) {
					List_Add(pList, newNode(AreaPointArray[(robotArea + i + 1) % 4].x, AreaPointArray[(robotArea + i + 1) % 4].y), NULL);
				}
			}
		}
	}
	List_Add(pList, newNode(pTarPose->x, pTarPose->y), NULL);
	return pList;
}
bool listAddTailRules(void const * arg1, void const * arg2)
{
	return false;
}
Type_List_Def * InterpolationPath(Type_List_Def * pList, Type_Node_Def *pStartPoint, Type_Node_Def * pEndPoint, int step)
{
	if (pList && pStartPoint && pEndPoint){
		
		Type_Vector3_Def startPos, endPos;
		startPos.x = pStartPoint->x;
		startPos.y = pStartPoint->y;
		
		endPos.x = pEndPoint->x;
		endPos.y = pEndPoint->y;
		
		int nums = (int)(caclDistance(&startPos, &endPos) / step);
		if (nums > 0) {
			Type_Vector3_Def inc;
			inc.x = pEndPoint->x - pStartPoint->x;
			inc.y = pEndPoint->y - pStartPoint->y;

			List_Add(pList, newNode(pStartPoint->x, pStartPoint->y), listAddTailRules);
			for (int i = 1; i < nums; i++) {
				List_Add(pList, newNode(inc.x * ((float)i / nums) + pStartPoint->x, inc.y * ((float)i / nums) + pStartPoint->y), listAddTailRules);
			}
			return pList;
		}
	}
	return NULL;
}
Type_List_Def * MovementPathGeneration(Type_List_Def * pathList)
{
	if (pathList){
		Type_List_Def * pRetList = newList();
		
		for (int i = 1; i < pathList->count; i++) {
			Type_List_Def *pList = newList();
			InterpolationPath(pList, List_ReadbyId( pathList, i - 1), List_ReadbyId( pathList, i), 100);
			for (int j = 0; j < pList->count; j++) {
				List_Add(pRetList, List_ReadbyId(pList, j), listAddTailRules);
			}
			ListDistroy(pList);
		}
		ListDistroy(pathList);
		return pRetList;
	}else{
		return NULL;
	}
}
int xp_joint_get_wheel_encode(uint64_t encBuf[2])
{
	return xp_wheel_get_encode(encBuf);
}

int xp_joint_get_wheel_encode_inc(int64_t errBuf[2])
{
	int64_t CurEncodeBuf[2] = {0};

	if (0 == xp_joint_get_wheel_encode((uint64_t *)CurEncodeBuf)) {

		for (int i = 0; i < 2; i++) {

			errBuf[i] = CurEncodeBuf[i] - Link.LastEncodeBuf[i];

			if (errBuf[i] >= MAX_DISTANCE_PER_PERIOD) {
				errBuf[i] -= MAX_ENCODE_RANGE;
			}
			else if (errBuf[i] <= -MAX_DISTANCE_PER_PERIOD) {
				errBuf[i] += MAX_ENCODE_RANGE;
			}
			Link.LastEncodeBuf[i] = CurEncodeBuf[i];
		}
		return 0;
	}
	else {
		return 1;
	}
}
//这里对于连杆的定义,序号0的关节表示管线架中心以及连接它的的第一个连杆
int xp_joint_update_pos(uint8_t type)
{
	//关节逆解(坐标->角度)
	/**********************************************************************************************/
	if (USED_ODOM == type) {
		int64_t errBuf[2] = { 0, 0 };
		float angle_update = 0;
#if true
	//通过左右轮编码器进行里程计计算
	/*************************************************************************/
	if (0 == xp_joint_get_wheel_encode_inc(errBuf)) {
		errBuf[0] *= PLUS2DIST_MM;
		errBuf[1] *= PLUS2DIST_MM;
		angle_update = (float)(errBuf[1] - errBuf[0]) / WHEEL_BASE + Link.robotPose.z * DEG2RAD;
	}else{
        return -1;
    }
	/*************************************************************************/
#else
        //通过左右轮速度对时间积分计算里程计
		//时间增量单位mm，速度单位m/s，物理尺寸单位mm
		float dt = aos_now_ms() - poseUpdateTimeStamp;
		float angle_update = dt * (vr - vl) / WHEELBASE + robotPose.z * Mathf.Deg2Rad;

		errBuf[0] = (vl * dt / MODULUS);
		errBuf[1] = (vr * dt / MODULUS);
#endif

		float dist = (float)(errBuf[0] + errBuf[1]) / 2.0f * PLUS2DIST_MM;

		Link.millage += fabsf(dist);

        //这里的Points[0]为坐标原点,在逆解算法中用到,结束后会变换回来的
        Type_Vector3_Def Points[4] = {0};

        Points[3].x = Link.joints[2].pose.x + cosf(angle_update) * dist;
        Points[3].y = Link.joints[2].pose.y + sinf(angle_update) * dist;

        //为首关节末端位置赋初值,加快逆解算法速度
        Points[1] = Link.joints[0].pose;

        //由机器中心计算管线架竖直杆旋转轴处坐标
        Points[2].x = (float)(Points[3].x + (ROBOT_CENTER_OFFSET_X) * cosf(angle_update) - (ROBOT_CENTER_OFFSET_Y) * sinf(angle_update));
        Points[2].y = (float)(Points[3].y + (ROBOT_CENTER_OFFSET_Y) * cosf(angle_update) + (ROBOT_CENTER_OFFSET_X) * sinf(angle_update));
        
        IkCacl(&Points[2], Points);

        float angle_sum = 0;
        for (int i = 0; i < JOINT_NUM; i++) {
            Link.joints[i].pose = Points[i];
            angle_sum += Link.joints[i].pose.z;
        }
        Link.robotPose.x = Link.joints[2].pose.x;
        Link.robotPose.y = Link.joints[2].pose.y;
        Link.robotPose.z = fmodf(angle_sum - ROBOT_CENTER_ANGLE + 360 + 180, 360.0f);


	}
	else if (USED_ENC) {
		float angle_sum = 0;
		float x_sum = 0, y_sum = 0;
		//关节正解（角度->坐标）
		/**********************************************************************************************/
		for (int i = 0; i < JOINT_NUM; i++) {

			angle_sum += Link.joints[i].pose.z;

			Link.joints[i].pose.x = (Link.joints[i].length * cosf(angle_sum * DEG2RAD) + x_sum);
			Link.joints[i].pose.y = (Link.joints[i].length * sinf(angle_sum * DEG2RAD) + y_sum);

			x_sum = Link.joints[i].pose.x;
			y_sum = Link.joints[i].pose.y;

		}

		Link.robotPose.x = Link.joints[JOINT_NUM - 1].pose.x;
		Link.robotPose.y = Link.joints[JOINT_NUM - 1].pose.y;
		Link.robotPose.z = fmodf(angle_sum - ROBOT_CENTER_ANGLE + 360 + 180, 360.0f);

		Link.millage += caclDistance(&Link.robotPose, &Link.robotLastPose);
		Link.robotLastPose = Link.robotPose;
		/**********************************************************************************************/
	}
	/* 设置机器人坐标，传递给雷达算法程序 */
	xp_linux_lidar_location_set((int)Link.robotPose.x, (int)Link.robotPose.y, Link.robotPose.z);
	
	xp_joint_electric_fence_check_polling();
	
	Link.poseUpdateTimeStamp = aos_now_ms();
	return 0;
}
uint8_t xp_joint_over_range_check(void)
{
	//计算机器人轮廓四个顶点在场地下的坐标
	/**********************************************************************************************/
	Type_Vector3_Def point = {0};	//机器人坐标系下的坐标点
	
	point.x = ROBOT_LENGTH / 2;
	point.y = ROBOT_WIDTH / 2;
	xp_joint_coordinate_transform_from_robot_to_world(&point, &RobotFramePointArray[0]);	//左前
	
	point.x = -ROBOT_LENGTH / 2;
	point.y = ROBOT_WIDTH / 2;
	xp_joint_coordinate_transform_from_robot_to_world(&point, &RobotFramePointArray[1]);	//左后
	
	point.x = -ROBOT_LENGTH / 2;
	point.y = -ROBOT_WIDTH / 2;
	xp_joint_coordinate_transform_from_robot_to_world(&point, &RobotFramePointArray[2]);	//右后
	
	point.x = ROBOT_LENGTH / 2;
	point.y = -ROBOT_WIDTH / 2;
	xp_joint_coordinate_transform_from_robot_to_world(&point, &RobotFramePointArray[3]);	//右前
	/**********************************************************************************************/
	
	uint8_t retStateBit = 0;
	static uint8_t retStateBit_Old = 0;

	for (uint8_t i = 0; i < sizeof(RobotFramePointArray) / sizeof(Type_Vector3_Def); i++){
		if (!isValidWordPose2(&RobotFramePointArray[i])){
				retStateBit |= 0x01 << i;
			}
	}

    Type_Vector3_Def pose = {0};

    if (caclDistance(&Link.joints[1].pose, &pose) < Link.minRadiusLimit){
		retStateBit |= BIT_MIN_RADIUS;
	}
	if (caclDistance(&Link.joints[1].pose, &pose) > Link.maxRadiusLimit){
		retStateBit |= BIT_MAX_RADIUS;
	}
    if (Link.joints[2].pose.z < Link.encode3LimitMin + ROBOT_CENTER_ANGLE){
		retStateBit |= BIT_ENC3_MIN;
	}
	if (Link.joints[2].pose.z > Link.encode3LimitMax + ROBOT_CENTER_ANGLE){
		retStateBit |= BIT_ENC3_MAX;
	}

    Link.efenceState = retStateBit & retStateBit_Old;
	retStateBit_Old = retStateBit;

	return Link.efenceState;
}
uint8_t xp_joint_get_efence_state(void)
{
	return Link.efenceState;
}
void xp_joint_print_efence_state(uint8_t state)
{
    print("Over range: ");
    for (int i = 0; i < 8; i++){
        if ((0x01 << i) & state){
            print(" %s", efenceStrList[i]);
        }
    }
    print("\r\n");
}

uint8_t xp_joint_electric_fence_check_polling(void)
{
	int speedL, speedR;
	float vl, vr;
	
	bool isCmdMove = xp_wheel_get_wheel_speed(&vl, &vr);
	//预测0.2s后机器各关节轴状态
	Predict_robot_state_by_speed(vl, vr, 0.2f);

	//失能电子围栏时解除轮锁
	if (!Link.Efence_en){
        if (xp_wheel_locker_get() == wheel_locker_rang_max){
            xp_wheel_locker_set(wheel_locker_release);
        }
        return 0;
    }
	uint8_t ret = xp_joint_over_range_check();
    //未触发电子围栏时解除轮锁
    if (!ret){
        if (xp_wheel_locker_get() == wheel_locker_rang_max){
            xp_wheel_locker_set(wheel_locker_release);
        }
        return 0;
    }

    xp_joint_print_efence_state(ret);
	
    speedL = (int)(vl * 100);
	speedR = (int)(vr * 100);

    //在有速度命令时对其影响进行判断
	if (isCmdMove){

        bool isUnlock1 = true;

        if (ret & (BIT_LF | BIT_LB | BIT_RB | BIT_RF)){
            isUnlock1 = false;
            //判断当前命令速度对外边框的影响
            switch (ret & (BIT_LF | BIT_LB | BIT_RB | BIT_RF))
            {
                //机器在: 仅左前 仅右前出线或者左前右前都出线时 允许: 左后 直后 右后退
                case BIT_LF:
                case BIT_RF:
                case BIT_LF | BIT_RF:
                    if (speedL < 0 || speedR < 0){
                        isUnlock1 = true;
                    }
                break;
                //机器在: 仅左后 仅右后出线或者左后右后都出线时 允许: 左前 直前 右前进	
                case BIT_LB:
                case BIT_RB:
                case BIT_LB | BIT_RB:
                    if (speedL > 0 || speedR > 0){
                        isUnlock1 = true;
                    }
                break;
                    
                //机器在: 左前左后出线时 允许: 右后退 右前进
                case BIT_LF | BIT_LB:
                    if (abs(speedL) > abs(speedR)){
                        isUnlock1 = true;
                    }
                break;
                //机器在: 右前右后出线时 允许: 左后退 左前进
                case BIT_RF | BIT_RB:
                    if (abs(speedL) < abs(speedR)){
                        isUnlock1 = true;
                    }
                break;
                    
                //机器在: 左前右前右后出线时 允许: 左后退
                case BIT_LF | BIT_RF | BIT_RB:
                    if (speedR < 0 && speedR < speedL){
                        isUnlock1 = true;
                    }
                break;
                //机器在: 左前左后右前出线时 允许: 右后退
                case BIT_LF | BIT_LB | BIT_RF:
                    if (speedL < 0 && speedL < speedR){
                        isUnlock1 = true;
                    }
                break;
                //机器在: 左后右后右前出线时 允许: 左前进
                case BIT_LB | BIT_RB | BIT_RF:
                    if (speedR > 0 && speedR > speedL){
                        isUnlock1 = true;
                    }
                break;
                //机器在: 左前左后右后出线时 允许: 右前进
                case BIT_LF | BIT_LB | BIT_RB:
                    if (speedL > 0 && speedL > speedR){
                        isUnlock1 = true;
                    }
                break;
                
                //机器在: 左前左后右前右后出线时 允许: 都出线了,随意吧
                case BIT_LF | BIT_RF | BIT_LB | BIT_RB:
                    isUnlock1 = true;
                break;
                    //除以上状态外,乖乖给我停下
                default:
                    isUnlock1 = false;
            }
        }
        

        bool isUnlock2 = true;
        if (ret & (BIT_MIN_RADIUS | BIT_MAX_RADIUS | BIT_ENC3_MIN | BIT_ENC3_MAX)){
            isUnlock2 = false;
            Type_Vector3_Def pose = {0};
            if (ret & BIT_MIN_RADIUS){
                /*if (caclDistance(&Link.joints[1].nextPose, &pose) > caclDistance(&Link.joints[1].pose, &pose)) {
                    isUnlock2 = true;
                    print("unlock (%d,%d) (%d,%d)", (int)Link.joints[1].nextPose.x, (int)Link.joints[1].nextPose.y, (int)Link.joints[1].pose.x, (int)Link.joints[1].pose.y);
                }*/
                if (speedL < 0 && speedR < 0){
                    isUnlock2 = true;
                }
            }
            if (ret & BIT_MAX_RADIUS){
                if (caclDistance(&Link.joints[1].nextPose, &pose) < caclDistance(&Link.joints[1].pose, &pose)) {
                    isUnlock2 = true;
                }
            }
            if (ret & BIT_ENC3_MIN){
                //if (Link.joints[2].nextPose.z > Link.joints[1].pose.z){
                if (speedL < 0 && speedR > 0){
                    isUnlock2 = true;
                }
            }
            if (ret & BIT_ENC3_MAX){
                //if (Link.joints[2].nextPose.z < Link.joints[1].pose.z){
                if (speedL > 0 && speedR < 0){
                    isUnlock2 = true;
                }
            }
        }
        //仅当外框和内保护都允许解锁时才能解除轮锁
		if (isUnlock1 && isUnlock2){
			xp_wheel_locker_set(wheel_locker_release);
		}else{
			xp_wheel_locker_set(wheel_locker_rang_max);
		}
	}
    return 0;
}
int xp_move_pid(float dist_err, float angle_err)
{
	float k = fabsf(fabsf(angle_err) - 90) * 0.011111f;
	
	Link.Pid_dist.out_u = xp_pid_cacl(&Link.Pid_dist, dist_err) * k;
	Link.Pid_angle.out_u = xp_pid_cacl(&Link.Pid_angle, angle_err);
	
	Link.vl = Link.Pid_dist.out_u - Link.Pid_angle.out_u;
	Link.vr = Link.Pid_dist.out_u + Link.Pid_angle.out_u;
	if (Link.motorEn){
		xp_wheel_set_wheel_speed(Link.vl, Link.vr);
	}
	/*if (Link.logTime){
		print("Pid_dist.out: %.2f Pid_angle.out: %.2f  vl:%.2f  vr:%.2f\r\n", 	Link.Pid_dist.out_u, \
																				Link.Pid_angle.out_u, \
																				Link.vl, \
																				Link.vr);
	}*/
}
int xp_joint_posture_adjust(float tarAngle)
{
	float err_angle = tarAngle - Link.robotPose.z;
	if (err_angle > 180) {
		err_angle -= 360;
	}
	if (err_angle < -180) {
		err_angle += 360;
	}

	if (fabsf(err_angle) < Link.angleAllowError) {
		return 1;
	}
	else {
		xp_move_pid(0, err_angle);
	}
	return 0;
}
//移动到目标点
int xp_joint_go_pose(Type_Vector3_Def const * pTarPose)
{

	//计算机器人到目标点的距离,用于速度调节和移动完成判断
	float dist_err = caclDistance( &Link.robotPose, pTarPose );

	//距离误差小于允许误差后完成移动任务
	if (dist_err < Link.posAllowError) {
		Link.motionProgress = 100;
		return 1;
	}
	else {
		//这里计算一个移动完成进度,用于路径规划对下一个位置点选取的判断
		Link.motionProgress = 100 - limit_float(dist_err, 0, Link.motionDistance) / Link.motionDistance * 100;
	}

	Type_Vector3_Def pose = {0};
	Type_Vector3_Def origin = {0};

	float angle_err = caclAngle(&origin, xp_joint_coordinate_transform_from_world_to_robot(pTarPose, &pose));
	
	if (angle_err > 90 || angle_err < -90) {

		dist_err = -dist_err;
		if (angle_err > 90) {
			angle_err -= 180;
		}

		if (angle_err < -90) {
			angle_err += 180;
		}
	}
	if (angle_err > 0 && Link.joints[2].pose.z + angle_err > Link.encode3LimitMax + ROBOT_CENTER_ANGLE) {
		angle_err -= 180;
	}
	if (angle_err < 0 && Link.joints[2].pose.z + angle_err < Link.encode3LimitMin + ROBOT_CENTER_ANGLE) {
		angle_err += 180;
	}

	/*if (0 != Link.logTime) {
		//仅在设置了logtime时输出调试数据
		println("car angle: %.2f  err_theat: %.2f  err_dist: %.2f  map_x: %.2f  map_y: %.2f\r\n", Link.robotPose.z, angle_err, dist_err, pose.x, pose.y);
	}*/

	//进行角度,距离并行结构的PID计算
	xp_move_pid(dist_err, angle_err);
	return 0;
}
//获取两角度间最小差值
float xp_joint_get_angle_diff(float angle1, float angle2)
{
	float err_angle = angle1 - angle2;
	if (err_angle > 180) {
		err_angle -= 360;
	}
	if (err_angle < -180) {
		err_angle += 360;
	}
	return err_angle;
}
int xp_joint_get_area(Type_Vector3_Def const * pose)
{
	int state = 0;
	//在正X轴向的垂直区域内
	if (pose->x > Link.areaMinLimit_P.x && pose->x < Link.areaMaxLimit_P.x) {
		state |= IS_X_POSITIVE;
	}
	//在负X轴向的垂直区域内
	if (pose->x < Link.areaMinLimit_N.x && pose->x > Link.areaMaxLimit_N.x) {
		state |= IS_X_NEGATIVE;
	}
	//在正Y轴向的垂直区域内
	if (pose->y > Link.areaMinLimit_P.y && pose->y < Link.areaMaxLimit_P.y) {
		state |= IS_Y_POSITIVE;
	}
	//在负pose.y轴向的垂直区域内
	if (pose->y < Link.areaMinLimit_N.y && pose->y > Link.areaMaxLimit_N.y) {
		state |= IS_Y_NEGATIVE;
	}

	switch (state) {
		case IS_X_POSITIVE | IS_Y_POSITIVE:
			if ((Link.robotPose.z < 315 && Link.robotPose.z >= 225) || (Link.robotPose.z >= 45 && Link.robotPose.z < 135)) {
				return 0;
			}
			else {
				return 1;
			}

		case IS_X_POSITIVE | IS_Y_NEGATIVE:
			if ((Link.robotPose.z < 315 && Link.robotPose.z >= 225) || (Link.robotPose.z >= 45 && Link.robotPose.z < 135)) {
				return 0;
			}
			else {
				return 3;
			}

		case IS_X_NEGATIVE | IS_Y_NEGATIVE:
			if ((Link.robotPose.z < 315 && Link.robotPose.z >= 225) || (Link.robotPose.z >= 45 && Link.robotPose.z < 135)) {
				return 2;
			}
			else {
				return 3;
			}

		case IS_X_NEGATIVE | IS_Y_POSITIVE:
			if ((Link.robotPose.z < 315 && Link.robotPose.z >= 225) || (Link.robotPose.z >= 45 && Link.robotPose.z < 135)) {
				return 2;
			}
			else {
				return 1;
			}

		case IS_X_POSITIVE:
			return 0;

		case IS_X_NEGATIVE:
			return 2;

		case IS_Y_POSITIVE:
			return 1;

		case IS_Y_NEGATIVE:
			return 3;

		default:

			break;

	}
	return -1;
}
//移动策略采用两个平行的PID共同作用,与目标点距离进行PID用于调节车速
//与目标点相对角度用于调节左右轮差速
int xp_joint_move_to_point(int32_t tar_x, int32_t tar_y, float tar_angle)
{
#if xp_config_enabled(WASH_ROOF_RECTANGULAR)
	//直角洗车顶方式暂不做电子围栏判断
	if (1) {
#else
	//对目标点进行简单的电子围栏判断(未考虑机身超出围栏)
	if (tar_x < Link.areaMaxLimit_P.x && tar_x > Link.areaMaxLimit_N.x && tar_y < Link.areaMaxLimit_P.y && tar_y > Link.areaMaxLimit_N.y) {
#endif
		Link.finallyTarPose.x = tar_x;
		Link.finallyTarPose.y = tar_y;
		Link.finallyTarPose.z = tar_angle;
		println("xp_joint_move_to_point: [%d,%d,%.1f]", tar_x, tar_y, tar_angle);
        while(xp_set_state(STA_PLAN)){
            aos_msleep(10);
        }
		return 0;
	}
	else {
		println("Target point over the place range\r\n");
		return -1;
	}
}
int xp_joint_turn_to_angle(float tar_angle)
{
	Link.finallyTarPose.z = tar_angle;
	println("xp_joint_turn_to_angle: %.1f", tar_angle);
    while(xp_set_state(STA_POSTURE)){
        aos_msleep(10);
    }
	return 0;
}
/*立即结束当前运动*/
int xp_joint_stop()
{
	return xp_set_state(STA_STOP);
}
/*暂停当前运动*/
int xp_joint_pause()
{
	return xp_set_state(STA_PAUSE);
}
/*恢复之前的运动*/
int xp_joint_resume()
{
	return xp_set_state(STA_RESUME);
}
Type_Vector3_Def* start_pose_seach(Type_Vector3_Def * const pPose)
{
    Type_Node_Def node;
	
    WorldPosToMapNode(&node, pPose);

    if (IsValidNode2(&node) && 0 != Link.Map[node.x][node.y]) {
        int px, nx, py, ny, index = 0;
        int min = INT16_MAX;

        for (px = node.x; px < Link.MAP_ROWS && 0 != Link.Map[px][node.y]; px++) ;
        if (px < Link.MAP_ROWS && min > px - node.x) {
            min = px - node.x;
            index = 1;
        }

        for (py = node.y; py < Link.MAP_COLUMNS && 0 != Link.Map[node.x][py]; py++) ;
        if (py < Link.MAP_COLUMNS && min > py - node.y) {
            min = py - node.y;
            index = 2;
        }

        for (nx = node.x; nx > 0 && 0 != Link.Map[nx][node.y]; nx--) ;
        if (nx > 0 && min > node.x - nx) {
            min = node.x - nx;
            index = 3;
        }

        for (ny = node.y; ny > 0 && 0 != Link.Map[node.x][ny]; ny--) ;
        if (ny > 0 && min > node.y - ny) {
            min = node.y - ny;
            index = 4;
        }

        switch (index) {
            case 1:
                node.x += min;
                break;
            case 2:
                node.y += min;
                break;
            case 3:
                node.x -= min;
                break;
            case 4:
                node.y -= min;
                break;
        }
    }
	
    return MapPosToWorldPos(&node, pPose);
}
int xp_joint_move_to_point_by_path_AStar(int32_t tar_x, int32_t tar_y, float tar_angle)
{
	//对目标点进行简单的电子围栏判断(未考虑机身超出围栏)
	if (tar_x < Link.areaMaxLimit_P.x && tar_x > Link.areaMaxLimit_N.x && tar_y < Link.areaMaxLimit_P.y && tar_y > Link.areaMaxLimit_N.y) {

		Type_Vector3_Def tarPose;
		tarPose.x = tar_x;
		tarPose.y = tar_y;
		tarPose.z = tar_angle;
		
		//为减少计算量,仅在进行位置规划时才进行Robot2的障碍点更新
		joint_robot2_obstacle_update(&Link.robot2Pose);
        println("Robot 2 pos: [%d %d]\r\n", (int)Link.robot2Pose.x, (int)Link.robot2Pose.y);

		Type_Vector3_Def pose = Link.robotPose;
        Type_Node_Def node;
        WorldPosToMapNode(&node, &pose);
        void *pList;

        if (IsValidNode2(&node) && 0 != Link.Map[node.x][node.y]) {
            Type_Vector3_Def poseSrc = {0}, poseDes1, poseDes2;
            poseSrc.x = ROBOT_LENGTH / 2;
            xp_joint_coordinate_transform_from_robot_to_world(&poseSrc, &poseDes1);
            poseSrc.x = -ROBOT_LENGTH / 2;
            xp_joint_coordinate_transform_from_robot_to_world(&poseSrc, &poseDes2);

            Type_List_Def *path1,*path2;
            path1 = SeachPathAStar(start_pose_seach(&poseDes1), &tarPose);
            path2 = SeachPathAStar(start_pose_seach(&poseDes2), &tarPose);

            if (NULL != path1 && NULL != path2) {
                if (path1->count <= path2->count){
                    pList = path1;
                    ListDistroy(path2);
                }else{
                    pList = path2;
                    ListDistroy(path1);
                }
            }
            else if (NULL != path1){
                pList = path1;
            }
            else if (NULL != path2) {
                pList = path2;
            }else{
                pList = NULL;
            }

        }else{
            pList = SeachPathAStar(&pose, &tarPose);
        }

		if (pList){
			println("Path generate successful. total point: %d\r\n", ((Type_List_Def *)pList)->count);
			
			while(!isMotionThreadIdle){
                aos_msleep(10);
            };		//等待motion状态机处于空闲状态,此函数块不能耗时
			if (Link.pPathList){
				ListDistroy(Link.pPathList);
				Link.pPathList = NULL;
			}
			
			Link.pPathList = pList;
			Link.finallyTarPose = tarPose;

			return xp_set_state(STA_PLAN);
		}else{
			println("Path generate failed! start:(%d, %d)  end:(%d, %d)\r\n", (int)pose.x, (int)pose.y, (int)tarPose.x, (int)tarPose.y);
		}
	}
	else {
		println("Target point over the place range\r\n");
		return -1;
	}
}
void joint_robot2_obstacle_update(Type_Vector3_Def const * pRobot2Pose)
{
    Type_Node_Def robotNode = {0};
	Type_Node_Def tmpNode = {0};
	uint16_t radius = ROBOT_LENGTH / MAP_GRID_WIDTH;
    
    WorldPosToMapNode(&robotNode, &Link.robot2MapPose);
	//清除上次的Robot2障碍
	for (int i = 0; i < 360; i++) {
		tmpNode.x = (int)(radius * cosf(i * DEG2RAD) + 0.5f) + robotNode.x;
		tmpNode.y = (int)(radius * sinf(i * DEG2RAD) + 0.5f) + robotNode.y;
		
		if (IsValidNode2(&tmpNode) && 3 == Link.Map[tmpNode.x][tmpNode.y]){
			Link.Map[tmpNode.x][tmpNode.y] = 0;
		}
		//这里在内层再增加一圈障碍点,因为绊脚问题,某些情况下可能会穿越障碍
		tmpNode.x = (int)((radius - 1) * cosf(i * DEG2RAD) + 0.5f) + robotNode.x;
		tmpNode.y = (int)((radius - 1) * sinf(i * DEG2RAD) + 0.5f) + robotNode.y;
		
		if (IsValidNode2(&tmpNode) && 3 == Link.Map[tmpNode.x][tmpNode.y]){
			Link.Map[tmpNode.x][tmpNode.y] = 0;
		}
	}
	//更新Robot2障碍位置
	Link.robot2MapPose = *pRobot2Pose;
	WorldPosToMapNode(&robotNode, &Link.robot2MapPose);
	//设置当前Robot2的最新障碍
	for (int i = 0; i < 360; i++) {
		tmpNode.x = (int)(radius * cosf(i * DEG2RAD) + 0.5f) + robotNode.x;
		tmpNode.y = (int)(radius * sinf(i * DEG2RAD) + 0.5f) + robotNode.y;
		
		if (IsValidNode2(&tmpNode) && 0 == Link.Map[tmpNode.x][tmpNode.y]){
			Link.Map[tmpNode.x][tmpNode.y] = 3;
		}
		//这里在内层再增加一圈障碍点,因为绊脚问题,某些情况下可能会穿越障碍
		tmpNode.x = (int)((radius - 1) * cosf(i * DEG2RAD) + 0.5f) + robotNode.x;
		tmpNode.y = (int)((radius - 1) * sinf(i * DEG2RAD) + 0.5f) + robotNode.y;
		
		if (IsValidNode2(&tmpNode) && 0 == Link.Map[tmpNode.x][tmpNode.y]){
			Link.Map[tmpNode.x][tmpNode.y] = 3;
		}
	}
}
int xp_joint_move_to_point_by_path_static_point(int32_t tar_x, int32_t tar_y, float tar_angle, Type_MotionOption_Enum option)
{
	//对目标点进行简单的电子围栏判断(未考虑机身超出围栏)
	if (tar_x < Link.areaMaxLimit_P.x && tar_x > Link.areaMaxLimit_N.x && tar_y < Link.areaMaxLimit_P.y && tar_y > Link.areaMaxLimit_N.y) {

		Type_Vector3_Def tarPose;
		tarPose.x = tar_x;
		tarPose.y = tar_y;
		tarPose.z = tar_angle;
		
		
		void *pPathList = MovementPathGeneration(MotionPlan(&tarPose, option));
		
		if (pPathList){
			println("Path generate successful. total point:%d\r\n", Link.pPathList->count);
			
			while(!isMotionThreadIdle);		//等待motion状态机处于空闲状态,此函数块不能耗时
			if (Link.pPathList){
				ListDistroy(Link.pPathList);
				Link.pPathList = NULL;
			}
			Link.pPathList = pPathList;
			Link.finallyTarPose = tarPose;
			return xp_set_state(STA_PLAN);
		}else{
			println("Path generate failed! start:(%d, %d)  end:(%d, %d)\r\n", Link.robotPose.x, Link.robotPose.y, (int)Link.finallyTarPose.x, (int)Link.finallyTarPose.y);
		}
	}
	else {
		println("Target point over the place range\r\n");
		return -1;
	}
}

/******************************************************************************/

Type_Vector3_Def * MapPosToWorldPos(Type_Node_Def * const pNode, Type_Vector3_Def * const pVector)
{
	pVector->x = pNode->x * MAP_GRID_WIDTH + Link.areaMaxLimit_N.x;
	pVector->y = pNode->y * MAP_GRID_WIDTH + Link.areaMaxLimit_N.y;

	return pVector;
}
//场地坐标转为地图位置
uint8_t *WorldPosToMapPtr(int x, int y)
{
	x = (x - Link.areaMaxLimit_N.x) / MAP_GRID_WIDTH;
	y = (y - Link.areaMaxLimit_N.y) / MAP_GRID_WIDTH;
	
	if (x < 0) { x = 0; }
	if (x > Link.MAP_ROWS - 1) { x = Link.MAP_ROWS - 1; }
	
	if (y < 0) { y = 0; }
	if (y > Link.MAP_COLUMNS - 1) { y = Link.MAP_COLUMNS - 1; }
	
	return (uint8_t *)&(Link.Map[x][y]);
}
//场地坐标转为地图引用
uint8_t *WorldPosToMapPtr2(Type_Vector3_Def const *pPose)
{
    Type_Node_Def node;
    WorldPosToMapNode(&node, pPose);

	return (uint8_t *)&(Link.Map[node.x][node.y]);
}
//场地坐标转为地图节点
Type_Node_Def *WorldPosToMapNode(Type_Node_Def * const pNode, Type_Vector3_Def const * pVector)
{
	if (pNode && pVector){
		pNode->x = (pVector->x - Link.areaMaxLimit_N.x) / MAP_GRID_WIDTH;
		pNode->y = (pVector->y - Link.areaMaxLimit_N.y) / MAP_GRID_WIDTH;
	}
	return pNode;
}

void mapInit_hasCar()
{
    Type_Vector3_Def pose;
	memset(Link.Map[0], 0, sizeof(uint8_t) * Link.MAP_ROWS * Link.MAP_COLUMNS);

	//将与X轴平行的场地两侧最大边框所对应的地图栅格设置为障碍
	for (int i = Link.areaMaxLimit_N.x; i < Link.areaMaxLimit_P.x; i++) {
		*WorldPosToMapPtr(i, Link.areaMaxLimit_N.y) = 1;
		*WorldPosToMapPtr(i, Link.areaMaxLimit_P.y) = 1;
	}
	
	//将与y轴平行的场地两侧最大边框所对应的地图栅格设置为障碍
	for (int i = Link.areaMaxLimit_N.y; i < Link.areaMaxLimit_P.y; i++) {
		*WorldPosToMapPtr(Link.areaMaxLimit_N.x, i) = 1;
		*WorldPosToMapPtr(Link.areaMaxLimit_P.x, i) = 1;
	}

	for (int x = Link.areaMinLimit_N.x; x < Link.areaMinLimit_P.x; x++) {
		for (int y = Link.areaMinLimit_N.y; y < Link.areaMinLimit_P.y; y++) {
			*WorldPosToMapPtr(x, y) = 1;
		}
	}
	DilationMap((ROBOT_WIDTH + ROBOT_LENGTH) / 4 / MAP_GRID_WIDTH);
}
//List 插入数据时的排序规则
bool SortRule_F_from_small_to_big(void const * pListNode, void const * pNode)
{
	Type_Node_Def const * _pListNode = (Type_Node_Def *)pListNode;
	Type_Node_Def const * _pNode = (Type_Node_Def *)pNode;
	
	if (_pNode->F < _pListNode->F || (_pNode->F == _pListNode->F && _pNode->H < _pListNode->H)){
		return true;
	}else{
		return false;
	}
}
//List 节点是否相等
bool isEqualRules(void const * pListNode, void const * pCurNode)
{
	if ( ((Type_Node_Def *)pListNode)->x == ((Type_Node_Def *)pCurNode)->x && ((Type_Node_Def *)pListNode)->y == ((Type_Node_Def *)pCurNode)->y){
		return true;
	}else{
		return false;
	}
}
//在list中获取花费最小的节点(由于在Add时已经根据花费进行了排序,这里直接返回头节点)
Type_Node_Def * const FindMinFCostNode(Type_List_Def const * pList)
{
	if (NULL != pList){
		return pList->head;
	}
	return NULL;
}
//找到终点后往前遍历出路径
Type_List_Def * GeneratePath(Type_Node_Def * const pStartNode, Type_Node_Def * const pEndNode)
{
	Type_List_Def * pPathList = newList();
	if (NULL != pPathList){
		Type_Node_Def * pNode = pEndNode;
        List_Add(pPathList, newNode(pNode->x, pNode->y), NULL);
		while (pNode->parent && !isEqualRules(pNode->parent, pStartNode)) {
			List_Add(pPathList, newNode(pNode->x, pNode->y), NULL);
			pNode = pNode->parent;
		}
		return ListReverse(pPathList); //翻转路径list
	}else{
		print("ERROR! memery malloc failed");
		return NULL;
	}
}
//判断是否是地图范围内的场地坐标
bool isValidWordPose(int x, int y)
{
    if (x >= Link.areaMaxLimit_N.x && x <= Link.areaMaxLimit_P.x && y >= Link.areaMaxLimit_N.y && y <= Link.areaMaxLimit_P.y) {
		return true;
	}
	return false;
}
//判断是否是地图范围内的场地坐标
bool isValidWordPose2(Type_Vector3_Def const *pPose)
{	
	return isValidWordPose(pPose->x, pPose->y);
}
//判断是否是地图范围内的节点
bool IsValidNode(int x, int y)
{	
	if (x >= 0 && x < Link.MAP_ROWS && y >= 0 && y < Link.MAP_COLUMNS) {
		return true;
	}
	return false;
}
//判断是否是地图范围内的节点
bool IsValidNode2(Type_Node_Def const *pNode)
{	
	return IsValidNode(pNode->x, pNode->y);
}
//获取指定节点周围的邻居节点列表
Type_List_Def * GetNeighor(Type_Node_Def const * pNode, Type_List_Def const * pCloseList)
{
	Type_List_Def * pNeighbor = newList();
	if (NULL != pNeighbor){
		for (int i = -1; i <= 1; i++) {
			for (int j = -1; j <= 1; j++) {
				if (i == 0 && j == 0) {
					continue;
				}
				//将除超出地图范围的点,障碍点,已存在闭列表的点添加到邻居列表
				if (IsValidNode(pNode->x + i, pNode->y + j) && 0 == Link.Map[pNode->x + i][pNode->y + j]) {
					Type_Node_Def *node = newNode(pNode->x + i, pNode->y + j);
					
					if (NULL == List_Find(pCloseList, node, isEqualRules)) {
						List_Add(pNeighbor, node, NULL);
					}else{
						xp_joint_memery_free(node);
					}
				}
			}
		}	
		return pNeighbor;
	}else{
		print("ERROR! memery malloc failed");
		return NULL;
	}
}
int GetDistance(Type_Node_Def const * pNode1, Type_Node_Def const * pNode2)
{
	int deltaX = abs(pNode1->x - pNode2->x);
	int deltaY = abs(pNode1->y - pNode2->y);
	
	if (deltaX > deltaY) {
		return deltaY * 14 + 10 * (deltaX - deltaY);
	}
	else {
		return deltaX * 14 + 10 * (deltaY - deltaX);
	}
}


//障碍物膨胀
void DilationMap(int radius)
{
	if (NULL != Link.Map) {
		for (int i = 0; i < Link.MAP_ROWS; i++) {
			for (int j = 0; j < Link.MAP_COLUMNS; j++) {
				if (Link.Map[i][j] == 1) {

					for (int n = 0; n < radius; n++) {
						if (IsValidNode(i + n, j - n) && 0 == Link.Map[i + n][j - n]) {
							Link.Map[i + n][j - n] = 2;
						}
						if (IsValidNode(i + n, j + n) && 0 == Link.Map[i + n][j + n]) {
							Link.Map[i + n][j + n] = 2;
						}
						if (IsValidNode(i - n, j - n) && 0 == Link.Map[i - n][j - n]) {
							Link.Map[i - n][j - n] = 2;
						}
						if (IsValidNode(i - n, j + n) && 0 == Link.Map[i - n][j + n]) {
							Link.Map[i - n][j + n] = 2;
						}
					}
				}
			}
		}
	}
}
Type_List_Def* SeachPathAStar(Type_Vector3_Def * pStartPose, Type_Vector3_Def * pEndPose)
{
	uint64_t startTime = aos_now_ms();
	uint32_t LoopCnt = 0;
	Type_Node_Def StartPosOnMap, EndPosOnMap;
	
	WorldPosToMapNode(&StartPosOnMap, pStartPose);
	WorldPosToMapNode(&EndPosOnMap, pEndPose);
	
    if (!IsValidNode2(&StartPosOnMap) || !IsValidNode2(&EndPosOnMap)){
        println("start pos or End pos over range of map[%d,%d], start:(%d,%d) end:(%d,%d)\r\n", Link.MAP_ROWS, \
                                                                                                Link.MAP_COLUMNS, \
                                                                                                StartPosOnMap.x, \
                                                                                                StartPosOnMap.y, \
                                                                                                EndPosOnMap.x, \
                                                                                                EndPosOnMap.y);
        return NULL;
    }

	Type_List_Def * pOpenList = newList();
	Type_List_Def * pCloseList = newList();

	if (NULL == pOpenList || NULL == pCloseList){
		println("ERROR! memery malloc failed");
		return NULL;
	}
	
	List_Add(pOpenList, newNode(StartPosOnMap.x, StartPosOnMap.y), SortRule_F_from_small_to_big);

	//开列表中无数据表示搜遍了全图没找到
	while (pOpenList->count > 0) {

		//取开列表中F最小的节点
		Type_Node_Def * pCurrNode = FindMinFCostNode(pOpenList);
		
		List_Remove(pOpenList, pCurrNode = newNodeCopy(pCurrNode), isEqualRules);
		List_Add(pCloseList, pCurrNode, NULL);
		
		if (pCurrNode->x == EndPosOnMap.x && pCurrNode->y == EndPosOnMap.y) {
			Type_List_Def *pPath = GeneratePath(&StartPosOnMap, pCurrNode);
			ListDistroy(pOpenList);		//找到路径后回收开列表所有内存
			ListDistroy(pCloseList);	//找到路径后回收闭列表所有内存
            aos_msleep(10);
			println("Seach success! time:%lld LoopCnt:%ld  point nums: %d\r\n", aos_now_ms() - startTime, LoopCnt, pPath->count);
			//通过父节点往前遍历出路径
			return pPath;
		}
		//处理当前节点周围的邻居节点,更新它们的F,G,H值
		Type_List_Def * pNeighbor = GetNeighor(pCurrNode, pCloseList);
		Type_Node_Def * pItem = pNeighbor->head;

		for (uint8_t i = 0; pItem && i < pNeighbor->count; i++){
			int G = pCurrNode->G + GetDistance(pCurrNode, pItem);

			Type_Node_Def *pNode = List_Find(pOpenList, pItem, isEqualRules);

			if (NULL != pNode){
				if (G < pNode->G) {
					pNode->G = G;
					pNode->F = pNode->G + pNode->H;
					pNode->parent = pCurrNode;
				}
			}else{
				pItem->G = G;
				pItem->H = GetDistance(pItem, &EndPosOnMap);
				pItem->F = pItem->G + pItem->H;
				pItem->parent = pCurrNode;
				List_Add(pOpenList, newNodeCopy(pItem), SortRule_F_from_small_to_big);
			}
			pItem = pItem->child;
			LoopCnt++;
		}
		ListDistroy(pNeighbor);		//回收邻居列表
	}
	
	ListDistroy(pOpenList);		//没找到路径后回收开列表所有内存
	ListDistroy(pCloseList);	//没找到路径后回收闭列表所有内存
	
	print("Seach failed! time:%lld  LoopCnt:%ld\r\n", aos_now_ms() - startTime, LoopCnt);
	return NULL;
}

/******************************************************************************/
void PrintList(Type_List_Def * const pList)
{
	Type_Node_Def * pItem = pList->head;
	print("Output List node x,y => ");
	while(NULL != pItem){
		print(" %d,%d", pItem->x, pItem->y);
		pItem = pItem->child;
	}
	print("\r\n");
}
void AStar_Test(void)
{
#if 0
	Type_List_Def * pTestList = newList();
	Type_Node_Def * pNode;
	
	
	List_Add(pTestList, newNode(5, 0), SortRule_F_from_small_to_big);
	List_Add(pTestList, pNode = newNode(3, 4), SortRule_F_from_small_to_big);
	List_Add(pTestList, newNode(7, 0), SortRule_F_from_small_to_big);
	List_Add(pTestList, newNode(3, 5), SortRule_F_from_small_to_big);
	List_Add(pTestList, newNode(3, 3), SortRule_F_from_small_to_big);
	
	PrintList(pTestList);
	
	int i = 0;
	Type_Node_Def *pRetNode = NULL;
	
	while( (pRetNode = List_ReadbyId(pTestList, i)) ){
		print("Node %d %d,%d  F:%d ->%d\r\n", i++, pRetNode->x, pRetNode->y, pRetNode->F, pRetNode->F += 10);
	}
	
	print("List_Remove: %d,%d\r\n", pNode->x, pNode->y);
	List_Remove(pTestList, pNode, isEqualRules);
	
	PrintList(pTestList);

	pNode = newNode(5, 0);
	print("List_Find: %d,%d\r\n", pNode->x, pNode->y);
	
	if (List_Find(pTestList, pNode, isEqualRules)){
		print("Find success\r\n");
		PrintList(pTestList);
		
	}else{
		print("Find failed\r\n");
	}
	xp_joint_memery_free(pNode);
	
	
	print("List_Reverse:\r\n");
	ListReverse(pTestList);
	PrintList(pTestList);
	
	print("List_ReadFromHead: x,y=>\r\n");
	Type_Node_Def node;
	while(pTestList->count > 0){
		if (List_ReadFromHead(pTestList, &node)){
			print(" %d,%d", node.x, node.y);
		}
	}
	
	print("ListDistroy:\r\n");
	ListDistroy(pTestList);
	
#endif
	
#if 1
	Type_Vector3_Def startPose, endPose;
	startPose.x = 0;
	startPose.y = -1875;
	
	endPose.x = -1000;
	endPose.y = -1875;

	Type_List_Def * pListPath = SeachPathAStar(&startPose, &endPose);
	if (pListPath){
		print("Serach path success\r\n");
		PrintList(pListPath);
		
		print("sliderWindowFilter %d\r\n", sliderWindowFilter(pListPath, 5));
		PrintList(pListPath);
		ListDistroy(pListPath);
	}else{
		print("Serach path failed\r\n");
	}
	print("memCnt %d\r\n", mallocCount);
#endif
}

//计算点到两点组成的直线的距离
float xp_joint_cacl_dist_point_to_line(Type_Vector3_Def const * pPoint, Type_Vector3_Def const * pLinePoint1, Type_Vector3_Def const * pLinePoint2)
{
	return fabsf( (pLinePoint1->y - pLinePoint2->y) * pPoint->x + (pLinePoint2->x - pLinePoint1->x) * pPoint->y + pLinePoint1->x * pLinePoint2->y - pLinePoint1->y * pLinePoint2->x ) / 
			sqrtf( powf(pLinePoint1->y - pLinePoint2->y, 2) + powf(pLinePoint1->x - pLinePoint2->x, 2) );
}
//从场地坐标系转换到机器人坐标系
Type_Vector3_Def * const xp_joint_coordinate_transform_from_world_to_robot(Type_Vector3_Def const * pSrcPoint, Type_Vector3_Def * const pDesPoint)
{
	Type_Vector3_Def srcPoint;
	memcpy(&srcPoint, pSrcPoint, sizeof(Type_Vector3_Def));
	//由场地坐标系转到其它坐标系时,属于逆向,因此平移和旋转都是逆向计算
	srcPoint.x -= Link.robotPose.x;
	srcPoint.y -= Link.robotPose.y;
	srcPoint.z = -Link.robotPose.z;

	pDesPoint->x = srcPoint.x * cosf(srcPoint.z * DEG2RAD) - srcPoint.y * sinf(srcPoint.z * DEG2RAD);
	pDesPoint->y = srcPoint.y * cosf(srcPoint.z * DEG2RAD) + srcPoint.x * sinf(srcPoint.z * DEG2RAD);

	return pDesPoint;
}
//从机器人坐标系转换到场地坐标系
Type_Vector3_Def * const xp_joint_coordinate_transform_from_robot_to_world(Type_Vector3_Def const * pSrcPoint, Type_Vector3_Def * const pDesPoint)
{
	return xp_joint_coordinate_transform(pSrcPoint, pDesPoint, &Link.robotPose);
}
//从雷达坐标系转换到机器人坐标系
Type_Vector3_Def * const xp_joint_coordinate_transform_from_lidar_to_robot(Type_Vector3_Def const * pSrcPoint, Type_Vector3_Def * const pDesPoint)
{
	Type_Vector3_Def lidarOffset = { ROBOT_LIDAR_OFFSET_X, ROBOT_LIDAR_OFFSET_Y, ROBOT_LIDAR_OFFSET_Z };

	return xp_joint_coordinate_transform(pSrcPoint, pDesPoint, &lidarOffset);
}
//从雷达坐标系转换到场地坐标系
Type_Vector3_Def * const xp_joint_coordinate_transform_from_lidar_to_world(Type_Vector3_Def const * pSrcPoint, Type_Vector3_Def * const pDesPoint)
{
	Type_Vector3_Def * pPoint = xp_joint_coordinate_transform_from_lidar_to_robot(pSrcPoint, pDesPoint);

	return xp_joint_coordinate_transform_from_robot_to_world(pPoint, pDesPoint);
}
//坐标系平移和旋转的变化
Type_Vector3_Def * const xp_joint_coordinate_transform(Type_Vector3_Def const * pSrcPoint, Type_Vector3_Def * const pDesPoint, Type_Vector3_Def const * offset)
{
	//旋转变换
	pDesPoint->x = pSrcPoint->x * cosf(offset->z * DEG2RAD) - pSrcPoint->y * sinf(offset->z * DEG2RAD);
	pDesPoint->y = pSrcPoint->y * cosf(offset->z * DEG2RAD) + pSrcPoint->x * sinf(offset->z * DEG2RAD);
	//平移变换
	pDesPoint->x += offset->x;
	pDesPoint->y += offset->y;
	pDesPoint->z = pSrcPoint->z + offset->z;
	return pDesPoint;
}
//计算两点之间的角度
float caclAngle(Type_Vector3_Def const * pSrcPose, Type_Vector3_Def const * pDesPose)
{
	return atan2f((pDesPose->y - pSrcPose->y), (pDesPose->x - pSrcPose->x)) * RAD2DEG;
}
//计算两点之间距离
float caclDistance(Type_Vector3_Def const * pSrcPose, Type_Vector3_Def const * pDesPose)
{
	return sqrtf(powf(pDesPose->x - pSrcPose->x, 2) + powf(pDesPose->y - pSrcPose->y, 2));
}
//计算两点之间的角度
float xp_cacl_point_angle(float src_x, float src_y, float des_x, float des_y)
{
	return atan2((des_y - src_y), (des_x - src_x)) * RAD2DEG;
}
//计算两点之间距离
float xp_cacl_point_dist(float src_x, float src_y, float des_x, float des_y)
{
	return sqrt( pow(des_x - src_x, 2) + pow(des_y - src_y, 2) );
}
float xp_pid_cacl(Type_Pid_Def * const pid, float err)
{
	pid->sum += err;
	
	if (pid->sum > pid->sum_limit){
		pid->sum = pid->sum_limit;
	}
	
	if (pid->sum < -pid->sum_limit){
		pid->sum = -pid->sum_limit;
	}
	
	pid->out_u = pid->kp * err + pid->ki * pid->sum + pid->kd * (pid->err_last - err);
	pid->err_last = err;
	
	if (pid->out_u > pid->out_limit){
		pid->out_u = pid->out_limit;
	}
	
	if (pid->out_u < -pid->out_limit){
		pid->out_u = -pid->out_limit;
	}
	return pid->out_u;
}
int xp_joint_set_pos_error(int err)
{
	if (err >= 0){
		Link.posAllowError = err;
		println("xp_joint_set_pos_error %.3f", Link.posAllowError);
	}
	return Link.posAllowError;
}
float xp_joint_set_angle_error(float err)
{
	if (err >= 0){
		Link.angleAllowError = err;
		println("xp_joint_set_angle_error %.3f", Link.angleAllowError);
	}
	return Link.angleAllowError;
}
//获取关节length参数字典
char *xp_joint_get_length_strKey(uint8_t id)
{
	static char Buf[20];

	sprintf(Buf, "jot_linkLen%d", id);
	return Buf;
}

//储存参数
int xp_joint_save_params(char *pKeyStr, int32_t val)
{
	char buf[15] = {0};
	
	if (NULL != pKeyStr){
		sprintf(buf, "%d", val);
		uint8_t tryTimes = 3;
		while(tryTimes--){
			if( 0 != aos_kv_set(pKeyStr, buf, strlen(buf), 1) ){
				println("Update kv fail. <%s>  tryTime %d\r\n", pKeyStr, 3 - tryTimes);
			}else{
				return 0;
			}
		}
	}
	return -1;
}

//读取参数,如果不存在则创建,并设置为默认值
int32_t xp_joint_read_creat_params(char *pKeyStr, int32_t defaultValue)
{
	char buf[15] = {0};
	int len = sizeof(buf);
	
	if (NULL != pKeyStr){
		uint8_t tryTimes = 3;
		while(tryTimes--){
			if (0 != aos_kv_get(pKeyStr, buf, &len)){
				println("Read kv fail. <%s>  tryTime %d\r\n", pKeyStr, 3 - tryTimes);
			}else{
				return atoi(buf);
			}
		}
		
		sprintf(buf, "%d", defaultValue);
		tryTimes = 3;
		
		while(tryTimes--){
			if( 0 != aos_kv_set(pKeyStr, buf, strlen(buf), 1) ){
				println("Update kv fail. <%s>  tryTime %d\r\n", pKeyStr, 3 - tryTimes);
			}else{
                println("Creat key: %s value: %d OK\r\n", pKeyStr, defaultValue);
				return defaultValue;
			}
		}
	}
	return defaultValue;
}

//获取状态字符串
const char *xp_get_state_str(Type_MotionState_Enum state)
{
	if (state < sizeof(StateStrList) / sizeof(StateStrList[0])){
		return StateStrList[state];
	}
}

//获取电子围栏的x负坐标
int xp_joint_get_fence_N_x(void)
{
	return Link.areaMaxLimit_N.x;
}

//获取电子围栏的y负坐标
int xp_joint_get_fence_N_y(void)
{
	return Link.areaMaxLimit_N.y;
}

//获取移动状态
int xp_joint_get_motion_state(void)
{
	return Link.motionState;
}
//获取移动状态字符串
const char * xp_joint_get_motion_state_str(void)
{
	return xp_get_state_str(Link.motionState);
}

//查询机器人移动是否完成
bool xp_joint_is_move_complete(void)
{
	return (STA_IDLE == Link.motionState);
}
//获取机器人位姿
int xp_joint_get_robot_pose(int32_t * const p_x, int32_t * const p_y, float * const p_angle)
{
	//返回机器人位姿
	if (NULL != p_x){
		*p_x = (int32_t)Link.robotPose.x;
	}
	if (NULL != p_y){
		*p_y = (int32_t)Link.robotPose.y;
	}
	if (NULL != p_angle){
		*p_angle = Link.robotPose.z;
	}
	//如果距上次更新超过允许的时间(3000),则返回1
	return (aos_now_ms() - Link.poseUpdateTimeStamp > ALLOW_UPDATE_TIME_ERR) ? (1) : (0);
}
Type_Vector3_Def const * xp_joint_get_home_pose(void)
{
	return &Link.homePose;
}
void xp_joint_updata_another_robot_pose(int x, int y, int z)
{
	Link.robot2Pose.x = x;
	Link.robot2Pose.y = y;
	Link.robot2Pose.z = z;
}
int xp_joint_set_area_limit(int x, int y, int offset, bool isMax)
{
	int ret = 0;
	
	if (x < 0 && y < 0){
		if (isMax){
			Link.areaMaxLimit_N.x = x - offset;
			Link.areaMaxLimit_N.y = y - offset;
			ret = xp_joint_save_params("jot_areaMaxLimit_Nx", Link.areaMaxLimit_N.x);
			ret = xp_joint_save_params("jot_areaMaxLimit_Ny", Link.areaMaxLimit_N.y);
			print("Set areaMaxLimit_N (%d, %d) %s\r\n", (int)Link.areaMaxLimit_N.x, (int)Link.areaMaxLimit_N.y, ret ? ("Fail") : ("OK"));
		}else{
			Link.areaMinLimit_N.x = x - offset;
			Link.areaMinLimit_N.y = y - offset;
			ret = xp_joint_save_params("jot_areaMinLimit_Nx", Link.areaMinLimit_N.x);
			ret = xp_joint_save_params("jot_areaMinLimit_Ny", Link.areaMinLimit_N.y);
			print("Set areaMinLimit_N (%d, %d) %s\r\n", (int)Link.areaMinLimit_N.x, (int)Link.areaMinLimit_N.y, ret ? ("Fail") : ("OK"));
		}
	}else if (x > 0 && y > 0){
		if (isMax){
			Link.areaMaxLimit_P.x = x + offset;
			Link.areaMaxLimit_P.y = y + offset;
			ret = xp_joint_save_params("jot_areaMaxLimit_Px", Link.areaMaxLimit_P.x);
			ret = xp_joint_save_params("jot_areaMaxLimit_Py", Link.areaMaxLimit_P.y);
			print("Set areaMaxLimit_P (%d, %d) %s\r\n", (int)Link.areaMaxLimit_P.x, (int)Link.areaMaxLimit_P.y, ret ? ("Fail") : ("OK"));
		}else{
			Link.areaMinLimit_P.x = x + offset;
			Link.areaMinLimit_P.y = y + offset;
			ret = xp_joint_save_params("jot_areaMinLimit_Px", Link.areaMinLimit_P.x);
			ret = xp_joint_save_params("jot_areaMinLimit_Py", Link.areaMinLimit_P.y);
			print("Set areaMinLimit_P (%d, %d) %s\r\n", (int)Link.areaMinLimit_P.x, (int)Link.areaMinLimit_P.y, ret ? ("Fail") : ("OK"));
		}
	}else{
		print("Set areaLimit fail. the x,y must be same > 0 or same < 0\r\n");
		return -1;
	}
	return 0;
}
int xp_joint_set_home_pose(int x, int y, int z)
{
	int ret = 0;
	Link.homePose.x = x;
	Link.homePose.y = y;
	Link.homePose.z = z;
	ret = xp_joint_save_params( "jot_homePos_x", Link.homePose.x);
	ret += xp_joint_save_params( "jot_homePos_y", Link.homePose.y);
	ret += xp_joint_save_params( "jot_homePos_z", Link.homePose.z);
	return ret;
}
//设置移动速度,单位m/s
float xp_joint_set_motion_speed(float speed)
{
	if (speed >= 0){
		Link.Pid_dist.out_limit = speed;
		println("xp_joint_set_motion_speed %.3f", Link.Pid_dist.out_limit);
	}
	return Link.Pid_dist.out_limit;
}
//获取目标点的完成进度
uint8_t xp_joint_get_motion_progress(void)
{
	return Link.motionProgress;
}
float limit_float(float val, float min, float max)
{
	if (val < min)	val = min;
	else if (val > max)	val = max;
	return val;
}
int32_t limit_int32(int32_t val, int32_t min, int32_t max)
{
	if (val < min)	val = min;
	else if (val > max)	val = max;
	return val;
}
int xp_joint_is_straight_to_axis(float axis_x, float axis_y)
{
	if (0 != axis_x){
		return fabs(Link.robotPose.x - axis_x) < Link.posAllowError ? (1) : (fabs(Link.robotPose.x) > fabs(axis_x) ? (1) : (0));
	}
	if (0 != axis_y){
		return fabs(Link.robotPose.y - axis_y) < Link.posAllowError ? (1) : (fabs(Link.robotPose.y) > fabs(axis_y) ? (1) : (0));
	}
}

bool xp_joint_calibration_link(Type_Vector3_Def * const pTarPose)
{
	bool isOK = false;
	
    Type_Vector3_Def Points[4] = {0};

	//为了加快逆解算法的收敛速度,这里根据目标点象限范围对各关节坐标和角度进行相应初始化
	if (pTarPose->x > 0 && pTarPose->y > 0){
		Points[1].z = 330;
		Points[2].z = 95;
		Points[3].z = 85;
	}
	else if (pTarPose->x > 0 && pTarPose->y < 0){
		Points[1].z = 270;
		Points[2].z = 85;
		Points[3].z = 145;
	}
	else if (pTarPose->x < 0 && pTarPose->y < 0){
		Points[1].z = 150;
		Points[2].z = 100;
		Points[3].z = 250;
	}
	else if (pTarPose->x < 0 && pTarPose->y > 0){
		Points[1].z = 80;
		Points[2].z = 110;
		Points[3].z = 320;
	}
	//首先根据预设的各关节角度,通过一次正解计算出各轴坐标
    float angle_sum = 0;
	float x_sum = 0, y_sum = 0;
	for (int i = 1; i < sizeof(Points) / sizeof(Points[0]); i++) {

		angle_sum += Points[i].z;

		Points[i].x = (Link.joints[i - 1].length * cosf(angle_sum * DEG2RAD) + x_sum);
		Points[i].y = (Link.joints[i - 1].length * sinf(angle_sum * DEG2RAD) + y_sum);

		x_sum = Points[i].x;
		y_sum = Points[i].y;
	}
    //再通过逆解计算出真实的各轴角度
    float err = IkCacl(&Points[2], Points);
	if (err < IK_ALLOW_ERROR){
        print("\r\n**************************\r\n");
		print("IK Calibration Result:\r\n");
		print("Input pose: (%d, %d, %d)\r\n", (int)pTarPose->x, (int)pTarPose->y, (int)pTarPose->z);
		print("IK dist error: %.4f\r\n", err);
		for (uint8_t i = 0; i < sizeof(Points) / sizeof(Points[0]); i++){
			print("joint_%d (%d, %d, %.2f)\r\n", i, (int)Points[i].x, (int)Points[i].y, Points[i].z);
		}
		print("\r\n**************************\r\n");
		xp_enc_cacl_offset(0, Points[0].z);
		xp_enc_cacl_offset(1, Points[1].z);
		xp_enc_cacl_offset(3, pTarPose->z);
		isOK = true;
	}
	return isOK;
}
void Predict_robot_state_by_speed(float vl, float vr, float dt)
{
    //这里的Points[0]为坐标原点,在逆解算法中用到,结束后会变换回来的
    Type_Vector3_Def Points[4] = {0};

    Points[3] = Link.robotPose;
    Points[1] = Link.joints[0].pose;

    //根据速度和Δt计算Δθ
    float update_theta = dt * (vr - vl) * 1000 / WHEEL_BASE + Link.robotPose.z * DEG2RAD;
    float dist = dt * (vl + vr) * 1000 / 2.0f;

    Points[3].x += cosf(update_theta) * dist;
    Points[3].y += sinf(update_theta) * dist;
    Points[3].z = fmod(update_theta * RAD2DEG + 360, 360);
    //由机器中心计算管线架竖直杆旋转轴处坐标
    Points[2].x = (float)(Points[3].x + (ROBOT_CENTER_OFFSET_X) * cosf(update_theta) - (ROBOT_CENTER_OFFSET_Y) * sinf(update_theta));
    Points[2].y = (float)(Points[3].y + (ROBOT_CENTER_OFFSET_Y) * cosf(update_theta) + (ROBOT_CENTER_OFFSET_X) * sinf(update_theta));
	
    IkCacl(&Points[2], Points);

    float angle_sum = 0;
    for (int i = 0; i < JOINT_NUM; i++) {
        Link.joints[i].nextPose = Points[i];
        angle_sum += Link.joints[i].nextPose.z;
    }
    Link.robotNextPose.x = Link.joints[2].nextPose.x;
	Link.robotNextPose.y = Link.joints[2].nextPose.y;
	Link.robotNextPose.z = fmodf(angle_sum - ROBOT_CENTER_ANGLE + 360 + 180, 360.0f);
}
float IkCacl(Type_Vector3_Def const *pTarPose, Type_Vector3_Def Points[])
{
    if (NULL == Points){
        return 1000;
    }
    int Lens[] = { Link.joints[0].length, Link.joints[1].length };
    float error;
    int n = 0, PointsLen = JOINT_NUM;

    do {
        //Forward
        Points[PointsLen - 1] = *pTarPose;
        for (int i = PointsLen - 2; i > 0; i--) {
            float a = Lens[i] / caclDistance(&Points[i], &Points[i + 1]);
            Points[i].x = ((1 - a) * Points[i + 1].x + a * Points[i].x);
            Points[i].y = ((1 - a) * Points[i + 1].y + a * Points[i].y);
        }
        //Back
        for (int i = 0; i < PointsLen - 1; i++) {
            float a = Lens[i] / caclDistance(&Points[i], &Points[i + 1]);
            Points[i + 1].x = ((1 - a) * Points[i].x + a * Points[i + 1].x);
            Points[i + 1].y = ((1 - a) * Points[i].y + a * Points[i + 1].y);
        }
        error = caclDistance(&Points[2], pTarPose);
    } while (error > IK_ALLOW_ERROR && n++ < IK_MAX_ITERATE_NUMS);

    float sum = 0;
    for (int i = 0; i < PointsLen; i++){
        sum += Points[i].z = fmod(caclAngle(&Points[i], &Points[i + 1]) - sum + 720, 360);
        Points[i].x = Points[i + 1].x;      //这里将连杆的定义顺序转换回原来
        Points[i].y = Points[i + 1].y;
    }
    return error;
}

void xp_joint_show_list(void)
{
	for (uint8_t i = 0; i < JOINT_NUM; i++){
		print("joints %d [angle:%.2f] [len:%d] [x:%.1f y:%.1f]\r\n", 	i, \
																	Link.joints[i].pose.z, \
																	Link.joints[i].length, \
																	Link.joints[i].pose.x, \
																	Link.joints[i].pose.y);
	}
}
//新建一个list头
Type_List_Def * const newList(void)
{
	Type_List_Def * pList = xp_joint_memery_malloc(sizeof(Type_List_Def));
	if (NULL != pList){
		pList->count = 0;
		pList->head = NULL;
		return pList;
	}else{
		print("ERROR! memery malloc failed");
		return NULL;
	}
}
void ListDistroy(Type_List_Def * pList)
{
	if (pList){
		Type_Node_Def * pItem = pList->head;
		Type_Node_Def * pLastItem;

		while(NULL != pItem){
			pLastItem = pItem;
			pItem = pItem->child;
			xp_joint_memery_free(pLastItem);
		}
		xp_joint_memery_free(pList);
		*(&pList) = NULL;
	}
}

//List 插入数据,带排序(排序规则由isSortRule规定)
bool List_Add(Type_List_Def * const pList, Type_Node_Def * const pNode, bool (*isMatchSortRules_CB)(void const * pListNode, void const * pNode))
{
	if (pList && pNode){
		
		Type_Node_Def ** p2Item = (Type_Node_Def **)&pList->head;
		
		while(*p2Item){
			if(isMatchSortRules_CB && isMatchSortRules_CB(*p2Item, pNode)) {
				break;
			}
			p2Item = &(*p2Item)->child;
		}
		
		pNode->child = *p2Item;
		*p2Item = pNode;
		
		pList->count++;
	}else{
		return false;
	}
	return true;
}
Type_Node_Def * const List_Find(Type_List_Def const * pList, Type_Node_Def const * pNode, bool (*isMatchFindRules_CB)(void const * pListNode, void const * pNode))
{
	if (pList && pNode && isMatchFindRules_CB){
		
		Type_Node_Def * pItem = pList->head;
		
		while(pItem){
			
			if ( isMatchFindRules_CB(pItem, pNode) ){
				return pItem;
			}
			pItem = pItem->child;
		}
	}
	return NULL;
}
bool List_Remove(Type_List_Def * const pList, Type_Node_Def const * pNode, bool (*isMatchRemoveRules_CB)(void const * pListNode, void const * pNode))
{
	Type_Node_Def * pItem = NULL;
	
	if (pList && pNode && isMatchRemoveRules_CB){
		
		Type_Node_Def ** p2Item = (Type_Node_Def **)&pList->head;
		
		while(*p2Item){
			
			pItem = *p2Item;
			
			if( isMatchRemoveRules_CB(pItem, pNode) ) {
				
				*p2Item = pItem->child;
				pList->count--;
				xp_joint_memery_free(pItem);
			}else{
				p2Item = &(*p2Item)->child;
			}
		}
	}else{
		return false;
	}
}

Type_Node_Def * List_ReadFromHead(Type_List_Def * const pList, Type_Node_Def * const pRetNode)
{
	if (pList && pList->head && pRetNode){
		Type_Node_Def * pItem = (Type_Node_Def *)pList->head;
		
		memcpy(pRetNode, pList->head, sizeof(Type_Node_Def));
		List_Remove(pList, pList->head, isEqualRules);
		return pRetNode;
	}
	return NULL;
}
Type_Node_Def * List_ReadbyId(Type_List_Def * const pList, uint16_t id)
{
	if (pList && pList->head && id < pList->count && id >= 0){
		
		Type_Node_Def * pNode = pList->head;
		while (id--) {
			pNode = pNode->child;
		}
		return pNode;
	}else{
		return NULL;
	}
}
Type_List_Def * const ListReverse(Type_List_Def * const pList)
{
	if (NULL != pList){
		Type_Node_Def * pItem_1 = pList->head;
		Type_Node_Def * pItem_2 = pItem_1->child;
		
		if (NULL == pItem_1 || NULL == pItem_2){
			return pList;
		}
		
		while(NULL != pItem_2){
			pItem_1->child = pItem_2->child;
			pItem_2->child = pList->head;
			pList->head = pItem_2;
			pItem_2 = pItem_1->child;
		}
	}
	return pList;
}

Type_Node_Def * const newNode(uint16_t x, uint16_t y)
{
	Type_Node_Def * node = xp_joint_memery_malloc(sizeof(Type_Node_Def));
	if (NULL != node){
		node->x = x;
		node->y = y;
		node->F = 0;
		node->G = 0;
		node->H = 0;
		node->parent = NULL;
		node->child = NULL;
		
		return node;
	}else{
		print("ERROR! memery malloc failed");
		return NULL;
	}
}
Type_Node_Def * const newNodeCopy(Type_Node_Def const * pNode)
{
	Type_Node_Def * node = xp_joint_memery_malloc(sizeof(Type_Node_Def));
	if (NULL != node){
		memcpy(node, pNode, sizeof(Type_Node_Def));
		return node;
	}else{
		print("ERROR! memery malloc failed");
		return NULL;
	}
}
int xp_joint_debug(char *type,char *fun,char *param)
{
	int ret = -1;
	
	//本调试接口仅支持 "joint_" 起始的命令
	if (NULL == strstr(type, "joint_")){
		return xp_angleEncode_debug(type, fun, param);
	}else{
		type += strlen("joint_");
	}
    //xp joint_listshow  打印所有关节相关信息
	if (strcasecmp(type, "ListShow") == 0){
		xp_joint_show_list();
	}
	//xp joint_setlength 0 2000 设置第一个连杆长度
	else if (strcasecmp(type, "setLength") == 0){
		uint8_t index = atoi(fun);
		uint16_t length = atoi(param);
		
		if ( index < JOINT_NUM && length != 0){
			Link.joints[index].length = length;
			xp_joint_save_params(xp_joint_get_length_strKey(index), Link.joints[index].length);
			print("Config joints %d length:%d\r\n", index, Link.joints[index].length);
		}else{
			print("Config param error!\r\n");
		}
	}
	//xp joint_logtime 200 (所有关节坐标,角度每200ms打印一次,默认为0不输出)
	else if (strcasecmp(type, "logTime") == 0){
		Link.logTime = atoi(fun);
		print("joints Log time: %d\r\n", Link.logTime);
	}
	//xp joint_cacldist -1829 846 计算车体当前位置离点(-1829 846)距离
	else if (strcasecmp(type, "caclDist") == 0){
		int32_t x = atoi(fun);
		int32_t y = atoi(param);
		print("Robot [x:%.1f y:%.1f] to tar[x:%.1f y:%.1f] dist:%.2f \r\n", Link.joints[1].pose.x, Link.joints[1].pose.y, x, y, xp_cacl_point_dist(Link.joints[1].pose.x, Link.joints[1].pose.y, x, y));
	}
	//xp joint_cacldist -1829 846 计算车体当前位置与点(-1829 846)角度
	else if (strcasecmp(type, "caclAngle") == 0){
		int32_t x = atoi(fun);
		int32_t y = atoi(param);
		print("Robot [x:%.1f y:%.1f] to tar[x:%.1f y:%.1f] angle:%.2f \r\n", Link.joints[1].pose.x, Link.joints[1].pose.y, x, y, xp_cacl_point_angle(Link.joints[1].pose.x, Link.joints[1].pose.y, x, y));
	}
	//xp joint_move -1829,846 180 移动到点(-1829 846)处,并且角度为180度
	else if (strcasecmp(type, "move") == 0){
		char *sp = NULL;
		char *p = NULL;
		int32_t x, y;

		if (NULL != (p = strtok_r(fun, "_", &sp))){
			x = atoi(p);
			if (NULL != (p = strtok_r(NULL, " ", &sp))){
				y = atoi(p);
				xp_joint_move_to_point(x, y, atof(param));
			}
		}
	}
	else if (strcasecmp(type, "movePath1") == 0){
		char *sp = NULL;
		char *p = NULL;
		int32_t x, y;

		if (NULL != (p = strtok_r(fun, "_", &sp))){
			x = atoi(p);
			if (NULL != (p = strtok_r(NULL, " ", &sp))){
				y = atoi(p);
				xp_joint_move_to_point_by_path_AStar(x, y, atof(param));
			}
		}
	}
	else if (strcasecmp(type, "movePath2_cw") == 0){
		char *sp = NULL;
		char *p = NULL;
		int32_t x, y;

		if (NULL != (p = strtok_r(fun, "_", &sp))){
			x = atoi(p);
			if (NULL != (p = strtok_r(NULL, " ", &sp))){
				y = atoi(p);
				xp_joint_move_to_point_by_path_static_point(x, y, atof(param), OPTION_DIRECT_CW);
			}
		}
	}
	else if (strcasecmp(type, "movePath2_ccw") == 0){
		char *sp = NULL;
		char *p = NULL;
		int32_t x, y;

		if (NULL != (p = strtok_r(fun, "_", &sp))){
			x = atoi(p);
			if (NULL != (p = strtok_r(NULL, " ", &sp))){
				y = atoi(p);
				xp_joint_move_to_point_by_path_static_point(x, y, atof(param), OPTION_DIRECT_CCW);
			}
		}
	}
	else if (strcasecmp(type, "movePath2_Auto") == 0){
		char *sp = NULL;
		char *p = NULL;
		int32_t x, y;

		if (NULL != (p = strtok_r(fun, "_", &sp))){
			x = atoi(p);
			if (NULL != (p = strtok_r(NULL, " ", &sp))){
				y = atoi(p);
				xp_joint_move_to_point_by_path_static_point(x, y, atof(param), OPTION_DIRECT_AUTO);
			}
		}
	}
	else if(strcasecmp(type, "homePose")==0){
        char *sp = NULL;
		char *p = NULL;
		int32_t x, y, z;

		if (NULL != (p = strtok_r(fun, "_", &sp))){
			x = atoi(p);
			if (NULL != (p = strtok_r(NULL, " ", &sp))){
				y = atoi(p);
				z = atoi(param);
				print("Home Pose from [%d, %d, %d] to [%d, %d, %d]\r\n", (int)Link.homePose.x, (int)Link.homePose.y, (int)Link.homePose.z, x, y, z);
				ret = xp_joint_set_home_pose(x, y, z);
				if (0 == ret){
					print("Home Pose set success! will reboot...");
					aos_msleep(2000);
					aos_reboot();
				}
			}
		}
    }
	else if (strcasecmp(type, "movestop") == 0){
		xp_set_state(STA_STOP);
	}
	else if (strcasecmp(type, "backhome") == 0){
		xp_joint_move_to_point_by_path_AStar(Link.homePose.x, Link.homePose.y, Link.homePose.z);
	}
	else if (strcasecmp(type, "motorEn") == 0){
		if ( atoi(fun) ){
			Link.motorEn = true;
		}else{
			Link.motorEn = false;
		}
		
		print("joints motion en_motor:%d\r\n", Link.motorEn);
	}
	else if (strcasecmp(type, "odomEn") == 0){
		if ( atoi(fun) ){
			Link.odomEn = true;
		}else{
			Link.odomEn = false;
		}
		
		print("joints motion odomEn:%d\r\n", Link.odomEn);
	}
	else if (strcasecmp(type, "Efence_en") == 0 || strcasecmp(type, "e_fence_en") == 0){
		if ( atoi(fun) ){
			Link.Efence_en = true;
			xp_joint_save_params("jot_Efence_en", Link.Efence_en);
		}else{
			Link.Efence_en = false;
			xp_joint_save_params("jot_Efence_en", Link.Efence_en);
			xp_wheel_locker_set(wheel_locker_release);
		}
		
		print("joints Efence_en:%d\r\n", Link.Efence_en);
	}
	else if (strcasecmp(type, "areaMaxLimit") == 0){
		xp_joint_set_area_limit(Link.robotPose.x, Link.robotPose.y, (ROBOT_LENGTH + ROBOT_WIDTH) / 4, true);
	}
	else if (strcasecmp(type, "areaMinLimit") == 0){
		xp_joint_set_area_limit(Link.robotPose.x, Link.robotPose.y, 0, false);
	}
	else if (strcasecmp(type, "setMaxLimit") == 0){
		char *sp = NULL;
		char *p = NULL;
		int32_t x, y;
		
		if (NULL != (p = strtok_r(fun, "_", &sp))){
			x = atoi(p);
			if (NULL != (p = strtok_r(NULL, " ", &sp))){
				y = atoi(p);
				xp_joint_set_area_limit(x, y, 0, true);
			}
		}
	}
	else if (strcasecmp(type, "setMinLimit") == 0){
		char *sp = NULL;
		char *p = NULL;
		int32_t x, y;
		
		if (NULL != (p = strtok_r(fun, "_", &sp))){
			x = atoi(p);
			if (NULL != (p = strtok_r(NULL, " ", &sp))){
				y = atoi(p);
				xp_joint_set_area_limit(x, y, 0, false);
			}
		}
	}
	else if (strcasecmp(type, "calibration") == 0){
		char *sp = NULL;
		char *p = NULL;
		Type_Vector3_Def pose;
		
		if (NULL != (p = strtok_r(fun, "_", &sp))){
			pose.x = atoi(p);
			if (NULL != (p = strtok_r(NULL, " ", &sp))){
				pose.y = atoi(p);
				pose.z = atof(param);
				xp_joint_calibration_link(&pose);
			}
		}
	}
	else if (strcasecmp(type, "setRadiusRange") == 0){
		Link.minRadiusLimit = atoi(fun);
		Link.maxRadiusLimit = atoi(param);
		xp_joint_save_params("jot_radius_min", Link.minRadiusLimit);
		xp_joint_save_params("jot_radius_max", Link.maxRadiusLimit);
		print("Link.RadiusLimitMin:%d  Link.RadiusLimitMax: %d\r\n", Link.minRadiusLimit, Link.maxRadiusLimit);
	}
	else if (strcasecmp(type, "setEnc3Range") == 0){
		Link.encode3LimitMin = atoi(fun);
		Link.encode3LimitMax = atoi(param);
		xp_joint_save_params("jot_enc3_min", Link.encode3LimitMin);
		xp_joint_save_params("jot_enc3_max", Link.encode3LimitMax);
		print("Link.encode3LimitMin:%d  Link.encode3LimitMax: %d\r\n", Link.encode3LimitMin, Link.encode3LimitMax);
	}
	else if (strcasecmp(type, "angle_kp") == 0){
		print("Link.Pid_angle.kp: %f->%f\r\n", Link.Pid_angle.kp, atof(fun));
		Link.Pid_angle.kp = atof(fun);
	}
	else if (strcasecmp(type, "angle_ki") == 0){
		print("Link.Pid_angle.ki: %f->%f\r\n", Link.Pid_angle.ki, atof(fun));
		Link.Pid_angle.ki = atof(fun);
	}
	else if (strcasecmp(type, "dist_kp") == 0){
		print("Link.Pid_dist.kp: %f->f\r\n", Link.Pid_dist.kp, atof(fun));
		Link.Pid_dist.kp = atof(fun);
	}
	else if (strcasecmp(type, "dist_ki") == 0){
		print("Link.Pid_dist.ki: %f->f\r\n", Link.Pid_dist.ki, atof(fun));
		Link.Pid_dist.ki = atof(fun);
	}
	else if (strcasecmp(type, "pos_error") == 0){
		print("Link.pos_error: %.2f->%.2f\r\n", Link.posAllowError, atof(fun));
		Link.posAllowError = atof(fun);
	}
	else if (strcasecmp(type, "angle_error") == 0){
		print("Link.angle_error: %.2f->%.2f\r\n", Link.angleAllowError, atof(fun));
		Link.angleAllowError = atof(fun);
	}
	else if (strcasecmp(type, "angle_out_max") == 0){
		print("Link.Pid_angle.out_limit: %.3f->%.3f\r\n", Link.Pid_angle.out_limit, atof(fun));
		Link.Pid_angle.out_limit = atof(fun);
	}
	else if (strcasecmp(type, "ForwardDist") == 0){
		print("Link.ForwardDist: %d->%d\r\n", Link.ForwardDist, atoi(fun));
		Link.ForwardDist = atoi(fun);
	}
	else if (strcasecmp(type, "dist_out_max") == 0){
		print("Link.Pid_dist.out_limit: %.3f->%.3f\r\n", Link.Pid_dist.out_limit, atof(fun));
		Link.Pid_dist.out_limit = atof(fun);
	}
	else if (strcasecmp(type, "AStarTest") == 0){
		AStar_Test();
	}
	else if (strcasecmp(type, "help") == 0){
		print("xp joint_[CMD] [--] [--]\r\n \
                CMD:\r\n \
                ListShow\r\n \
				setLength [id] [len mm]\r\n \
				logTime [time mm]\r\n \
				move [x]_[y] [z]\r\n \
                movePath1 [x]_[y] [z]\r\n \
                movePath2_cw [x]_[y] [z]\r\n \
                movePath2_ccw [x]_[y] [z]\r\n \
                movePath2_Auto [x]_[y] [z]\r\n \
                homePose [x]_[y] [z]\r\n \
				movestop\r\n \
                backhome\r\n \
				motorEn [bool 0/1]\r\n \
                odomEn [bool 0/1]\r\n \
                Efence_en/e_fence_en [bool 0/1]\r\n \
                areaMaxLimit\r\n \
                areaMinLimit\r\n \
                setMaxLimit [x]_[y]\r\n \
                setMinLimit [x]_[y]\r\n \
                calibration [x]_[y] [z]\r\n \
                setRadiusRange [min] [max]\r\n \
                setEnc3Range [min] [max]\r\n \
				angle_kp [float]\r\n \
                angle_ki [float]\r\n \
				dist_kp [float]\r\n \
                dist_ki [float]\r\n \
				pos_error [float]\r\n \
				angle_error [float]\r\n \
				angle_out_max [float]\r\n \
				dist_out_max [float]\r\n \
                ForwardDist [dist mm]\r\n \
                AStarTest\r\n \
				help\r\n");
	}
	return 1;
}

